\documentclass[letter,10pt]{article}

\usepackage[utf8]{inputenc} % allow utf-8 input
\usepackage[T1]{fontenc}    % use 8-bit T1 fonts
\usepackage[colorlinks=true]{hyperref}       % hyperlinks
\usepackage{url}            % simple URL typesetting
\usepackage{booktabs}       % professional-quality tables
\usepackage{amsfonts,amsmath,amsthm,amssymb,gensymb}       % blackboard math symbols
\usepackage{microtype}      % microtypography
\usepackage{nicefrac}       % compact symbols for 1/2, etc.
\usepackage{color}
\usepackage{lmodern}
\usepackage{multicol}
\usepackage[margin=1.0in]{geometry}

\newcommand{\SE}[1]{ \mathrm{SE(#1)} }
\newcommand{\SO}[1]{ \mathrm{SO(#1)} }
\newcommand{\real}{\mathbb{R}}
\newcommand{\refgroup}{\mathrm{ref}}
\newcommand{\Exp}{\mathrm{Exp}}
\newcommand{\Log}{\mathrm{Log}}
% \newcommand{\asym}[1]{{\lbrack #1\rbrack}^\wedge{}}
\newcommand{\asym}[1]{{\lbrack #1\rbrack}_\times{}}
% \newcommand{\asym}[1]{\widehat{\lbrack #1\rbrack}}

\newtheorem{theorem}{Theorem}[section]
\newtheorem{prop}{Proposition}[section]

\title{An On Manifold Design of EKF-based VIO:\\
Implementation Notes of XIVO}
\author{Xiaohan Fei\\
\texttt{feixh@cs.ucla.edu}}
\begin{document}

\maketitle

% \begin{abstract}
% \end{abstract}

\section{Model}
% We simply layout the structure of this manuscript here.
% Section~\ref{sect-model} presents the model used in EKF Visual-Inertial SLAM: from an idealized one to an implementation-friendly one by adding more and more realistic considerations.
% Section~\ref{sect-implementation} gives some implementation details including state representation, prediction \& update steps and initialization.
% Section~\ref{sect-discussion} lists some potential improvements.
% In the Appendix~\ref{sect-appendix}, we derive the analytical form of some Jacobians useful for implementation.

\subsection{Idealized model}
The model is the same as the one described in Section 2.1 of~\cite{jonesS10IJRR} of which the major results are briefly reviewed here. An idealized model to generate the sensed inertial $y_{imu}$ and visual data $y^i$ is given by:

\begin{equation}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{sb}^b(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=R_{sb}(t)v_{sb}^b(t), &T_{sb}(0)=0\\
\dot{v}_{sb}^b(t)=-\asym{\omega_{sb}^b(t)}v_{sb}^b(t)+\alpha_{sb}^b(t)\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big)\\
y_{imu}(t)=
\begin{bmatrix}
 \omega_m(t)\\
 \alpha_m(t)
\end{bmatrix}
\doteq
\begin{bmatrix}
	    \omega_{sb}^b(t)\\
	    \alpha_{sb}^b(t)-R_{sb}^\top(t)\gamma
           \end{bmatrix}.
\end{cases}
\label{eq-ideal-model}
\end{equation}

\begin{itemize}
 \item
$R_{sb}(t) \in \SO{3}$ and $T_{sb}(t) \in \real^3$ are the body to spatial frame rotation and translation, which form $g_{sb}(t)\doteq (R_{sb}(t), T_{sb}(t)) \in \SE{3}$ the rigid body transformation from the body frame to the spatial frame. We adopt the convention in~\cite{maSKS} for the subscripts, where $g_{ab}$ denotes a transformation from reference frame $b$ to $a$, such that for a point $X$ represented in $b$, $g_{ab} X$ brings it to frame $a$. \footnote{In other words, $T_{sb}$ is the position of the origin of the body frame with respect to the spatial frame with coordinates resolved in the spatial frame and $R_{sb}$ transforms coordinates resolved in the body frame to coordinates resolved in the spatial frame by multiplying on the left: $x^s = R_{sb}x^b$.}

\item
$\omega_{sb}^b(t), v_{sb}^b(t)\in \real^3$ are the body to spatial rotational and linear velocity measured in the body frame. $\alpha_{sb}^b(t)$ is the body to spatial linear acceleration measured in body frame. \footnote{In other words, they are, respectively, the rotational velocity, linear velocity, and linear acceleration of the body frame with respect to the spatial frame with coordinates resolved in the body frame.}

\item
The ``hat'' operator $\hat{\cdot}$ (or $\asym{\cdot}$ when the operand is an expression instead of a single variable) maps a vector $v \doteq [v_1, v_2, v_3]^\top \in \real^3$ to a skew-symmetric matrix
$$
\bar{v}=
\asym{v}=
\begin{bmatrix}
    0 & -v_3 & v_2 \\
    v_3 & 0 & -v_1 \\
    -v_2 & v_1 & 0
\end{bmatrix}
\doteq v_1 E_1 + v_2 E_2 + v_3 E_3$$
where $\{E_1, E_2, E_3\}$ forms a basis of ${\it so}(3)$ -- the Lie algebra of $\SO{3}$:

\begin{equation}
E_1=
\begin{bmatrix}
0 & 0 & 0 \\
0 & 0 & -1 \\
0 & 1 & 0
\end{bmatrix}
\quad
E_2=
\begin{bmatrix}
0 & 0 & 1 \\
0 & 0 & 0 \\
-1 & 0 & 0
\end{bmatrix}
\quad
E_3=
\begin{bmatrix}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}.
\end{equation}

\item
$X_0^i \in \real^3, i=1\cdots N$ denotes the set of static 3-D points in the spatial frame, which have trivial dynamics.

\item
$y^i(t) \in \real^2$ is the measurement of point $i$ at time $t$, which is the projection of the 3-D point in our case.

\item
$y_{imu}(t)$ is the inertial measurement, which consists of the instantaneous rotational velocity and linear acceleration measured in the body frame.

\item
$\gamma$ is gravity in spatial frame.

\item
$\pi: \real^3 \mapsto \real^2$ is the perspective projection.

\end{itemize}

\subsection{From idealized model to a realistic one}
Define new linear velocity $v_{sb}\doteq R_{sb}v_{sb}^b$ and then we have $\dot{T}_{sb}(t)=v_{sb}(t)$. Define $\dot{v}_{sb}(t)\doteq \alpha_{sb}(t)$ and then $\alpha_{sb}=R_{sb}\alpha_{sb}^b$. For consistency, also define $\omega_{sb}\doteq \omega_{sb}^b$. The model becomes:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{sb}(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)=\alpha_{sb}(t)\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big)\\
\begin{bmatrix}
 \omega_m(t)\\
 \alpha_m(t)
\end{bmatrix}
\doteq
\begin{bmatrix}
	    \omega_{sb}(t)\\
	    R_{sb}^\top(t) \big(\alpha_{sb}(t)-\gamma \big)
           \end{bmatrix}
+
           \begin{bmatrix}
           b_g\\
           b_a
           \end{bmatrix}.
\end{cases}
\end{equation*}

It is common practice to treat inertial measurements as inputs to the system (driving signals), thus we substitute $\omega_{sb}$ and $\alpha_{sb}$ with the actual inertial measurements:
\begin{equation*}
\begin{cases}
\omega_{sb}(t) = \omega_{m}-b_g\\
\alpha_{sb}(t) = R_{sb}(t)\big( \alpha_{m} - b_a\big) + \gamma
\end{cases}
\end{equation*}

and now the model is:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big( \alpha_m(t) - b_a(t) \big) + \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t)=\pi\big(R_{sb}^\top(t)(X_0^i-T_{sb}(t))\big).
\end{cases}
\end{equation*}

In the above model, we assume the camera frame coincides with the body frame, which usually is not true. We need to insert the camera to body transformation $g_{bc}\doteq (R_{bc}, T_{bc}) \in \SE{3}$, which is from an off-line calibration procedure, into the visual measurement process:

\begin{equation}
\begin{aligned}
y^i(t)
&= \pi \Big( g_{bc}^{-1} g_{sb}^{-1}(t)X_0^i \Big)\\
&= \pi \Big(R_{bc}^\top \Big( R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc} \Big)\Big).
\end{aligned}
\label{eq-vismeas}
\end{equation}

This camera to body alignment is usually available an off-line calibration procedure. However, high precision can be obtained if one treats the alignment states as unknown constants and estimate them on-line along with the ego-motion~\cite{li2012improving}. The alignment states have trivial dynamics since they are essentially constants:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=I\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big(\alpha_{m}(t) - b_a(t) \big) + \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{R}_{bc}(t)=0, &R_{bc}(0)=R_{bc}^{\text{calib}}\\
\dot{T}_{bc}(t)=0, &T_{bc}(0)=T_{bc}^{\text{calib}}\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t) = \pi \Big(R_{bc}^\top(t) R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc}(t) \Big).
\end{cases}
\end{equation*}

Also, the gravity is assumed perfectly known, which is not true in practice -- at least one needs to estimate the orientation of gravity if not both the magnitude and the orientation. Let $R_g(t)$ be the orientation correction of the gravity, which is $R_g(0)=I$ if we initialize the body frame to be gravity aligned:
\begin{equation}
 R_{sb}(0)\big( \alpha_{m}(0)-b_a(0)\big) + R_g(0) \gamma = 0.
\end{equation}
The above equation holds if one puts the platform still for a certain amount of time during initialization, typically several seconds. One can either use one good sample of inertial measurements or average all samples during the stationary period to obtain $\alpha_{m}(0)$ and solve for $R_{sb}(0)$, whose solution is $R_{sb}^{\text{init}}$. The model is:

\begin{equation*}
\begin{cases}
\dot{R}_{sb}(t)=R_{sb}(t)\asym{\omega_{m}(t)-b_g(t)}, &R_{sb}(0)=R_{sb}^{\text{init}}\\
\dot{T}_{sb}(t)=v_{sb}(t), &T_{sb}(0)=0\\
\dot{v}_{sb}(t)= R_{sb}(t)\big( \alpha_{m}(t) - b_a(t) \big) + R_g(t) \gamma\\
\dot b_g(t)=0, & b_g(0)=\omega_{bias}^{\text{calib}}\\
\dot b_a(t)=0, & b_a(0)=\alpha_{bias}^{\text{calib}}\\
\dot{R}_{bc}(t)=0, &R_{bc}(0)=R_{bc}^{\text{calib}}\\
\dot{T}_{bc}(t)=0, &T_{bc}(0)=T_{bc}^{\text{calib}}\\
\dot{R}_g(t)=0, &R_g(0) = I\\
\dot{X}_0^i=0, &i=1\cdots N\\
\text{Measurements:}\\
y^i(t) = \pi \Big(R_{bc}^\top(t) R_{sb}^\top(t)\big( X_0^i - T_{sb}(t)\big)-T_{bc}(t) \Big).
\end{cases}
\end{equation*}

\section{Error state}
\label{sect-model}

Adding noise to the measurement model and dropping the time variable we obtain the following model:

System dynamics:
\begin{equation}
 \begin{cases}
 \dot R = R \asym{\omega_m-b_g-n_g}\\
 \dot T = v\\
 \dot v = R(a_m-b_a-n_a) + R_g \gamma \\
 \dot b_g = 0 \\
 \dot b_a = 0 \\
 \dot R_{bc} = 0 \\
 \dot T_{bc} = 0
 \end{cases}
\end{equation}

Each component can be written as the composition of a nominal state and an error state. For variables defined in Euclidean space the composition is the usual addition such as $T=\bar T + \tilde T$; for rotation, the composition is defined as $R=\bar R R(\tilde \omega)$ where $R(\tilde \omega)\doteq \exp(\asym{\tilde \omega})$ maps the error vector $\tilde \omega \in \real^3$ to a rotation matrix $R(\tilde \omega) \in \SO{3}$. The exponential map can be approximated as follows when the magnitude of $\tilde \omega$ is small:

\begin{equation}
R(\tilde \omega) \doteq \exp(\asym{\tilde \omega})= I + \asym{\tilde \omega}
\end{equation}

Derivation of nominal and error state system dynamics is trivial for translation:
\begin{equation}
\begin{cases}
\dot{\bar T} &= \bar v\\
\dot{\tilde T} &= \tilde v
\end{cases}
\end{equation}

For rotation, it's more involved:

\begin{equation}
 \begin{aligned}
 \dot R &= R\asym{\omega_m-b_g-n_g} \\
 \frac{d}{dt}\bar R  R(\tilde \omega) &= \bar R R(\tilde \omega) \asym{\omega_m - (\bar b_g+\tilde b_g) - n_g} \\
 \frac{d}{dt}\bar R  (I+ \asym{\tilde\omega}) &= \bar R (I+ \asym{\tilde\omega}) \asym{(\omega_m - \bar b_g) + (-\tilde b_g-n_g)} \\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &= \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{(\omega_m-\bar b_g) + (-\tilde b_g - n_g)}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &= \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g} + \bar R \asym{\tilde \omega}\asym{-\tilde b_g - n_g}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
 &\approx \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g}\\
 \end{aligned}
 \label{eq-rotation-derivation}
\end{equation}

$\asym{\tilde \omega}\asym{-\tilde b_g - n_g}$ is the product of two error states/one error state and one noise term, which approximates zero and thus is dropped.

Now we have the evolving equation of the nominal \& error rotation state:
\begin{equation}
\begin{aligned}
 \dot{\bar R} &= \bar R\asym{\omega_m - \bar b_g} = \bar R \asym{\bar \omega}\\
\dot{\bar R}\asym{\tilde\omega} + \bar R \asym{\dot{\tilde \omega}}
&= \bar R \asym{-\tilde b_g - n_g} + \bar R\asym{\tilde\omega}\asym{\bar\omega}
\end{aligned}
\end{equation}
where $\bar \omega \doteq \omega_m - \bar b_g$.

Recall the identity $\dot R=R \asym{\omega}$, and thus $R^\top \dot R = \asym{\omega}$.

We left-multiple both sides of the last equation by $\bar R^\top$, and use the identity $\bar R^\top\dot{\bar R}=\asym{\bar\omega}$
\begin{equation}
\begin{aligned}
 \asym{\bar \omega}\asym{\tilde \omega} + \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g} + \asym{\tilde\omega}\asym{\bar\omega}\\
 \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g}
 + \big(\asym{\tilde\omega}\asym{\bar\omega} - \asym{\bar\omega}\asym{\tilde\omega}\big) \\
 \asym{\dot{\tilde\omega}} &= \asym{-\tilde b_g - n_g} + \asym{\big(\tilde\omega\times\bar\omega\big)} \quad\text{(recall} \asym{a\times b}=\asym{a}\asym{b}-\asym{b}\asym{a}\text{)}\\
\dot{\tilde\omega} &= -\tilde b_g - n_g + \tilde\omega\times\bar\omega \quad\text{(}\asym{\cdot} \text{ is linear)} \\
\dot{\tilde\omega} &= -\tilde b_g - n_g + \asym{\tilde\omega}\bar\omega \quad\text{(}a\times b=\asym{a}b \text{)}\\
\dot{\tilde\omega} &= -\tilde b_g - n_g - \asym{\bar\omega}\tilde\omega \quad\text{(}\asym{\cdot} \text{ is anti-commutative)}\\
\end{aligned}
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
 \dot{\bar R} &= \bar R \asym{\bar \omega}\\
\dot{\tilde\omega} &= - \asym{\bar\omega}\tilde\omega -\tilde b_g - n_g.
\end{cases}
\end{equation}

Last let's work on velocity term:

\begin{equation}
\begin{aligned}
 \dot v &= R(a_m-b_a-n_a) + R_g \gamma \\
 \dot{\bar v} + \dot{\tilde v} &= \bar R R(\tilde \omega) (a_m-(\bar b_a+\tilde b_a)-n_a) + \bar R_g(I + \asym{\tilde\omega_g}) \gamma \\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (I+\asym{\tilde\omega}) (a_m-(\bar b_a+\tilde b_a)-n_a) + \bar R_g \gamma + \bar R_g \asym{\tilde\omega_g} \gamma\\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (I+\asym{\tilde\omega}) \big( (a_m-\bar b_a) - (\tilde b_a+n_a) \big) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g\\
 \dot{\bar v} + \dot{\tilde v} &= \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a)- \bar R\asym{\tilde\omega}(\tilde b_a+n_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g \\
 \dot{\bar v} + \dot{\tilde v} &\approx \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g\\
\end{aligned}
 \label{eq-velocity-derivation}
\end{equation}

where nominal and error state equation can be defined as
\begin{equation}
\begin{cases}
\dot{\bar v} &= \bar R\bar a + \bar R_g \gamma\\
\dot{\tilde v} &= -\bar R(\tilde b_a + n_a) + \bar R \asym{\tilde\omega}\bar a - \bar R_g \asym{\gamma} \tilde\omega_g
\end{cases}
\end{equation}
where we have defined $\bar a \doteq a_m - \bar b_a$.

Note that $\asym{a}b=a\times b=-b\times a=-\asym{b} a$, we can write last equation

\begin{equation}
\dot{\tilde v}=-\bar R(\tilde b_a + n_a) - \bar R\asym{\bar a} \tilde \omega- \bar R_g \asym{\gamma} \tilde \omega_g.
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
\dot{\bar R} &= \bar R\asym{\bar \omega}\\
\dot{\bar T} &= \bar v\\
\dot{\bar v} &= \bar R \bar a + \bar R_g \gamma
\end{cases}
\label{eq:nominal-dynamics}
\end{equation}

and

\begin{equation}
\begin{bmatrix}
\dot{\tilde \omega} \\
\dot{\tilde T}\\
\dot{\tilde v}\\
\dot{\tilde b}_g\\
\dot{\tilde b}_a\\
\dot{\tilde \omega}_g
\end{bmatrix}
=
\begin{bmatrix}
-\asym{\bar\omega} & 0_{3\times3} & 0_{3\times3} & -I_{3\times3} & 0_{3\times3} & 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}\\
-\bar R\asym{\bar a} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & -\bar R& -\bar R_g \asym{\gamma}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}
\end{bmatrix}
\begin{bmatrix}
\tilde \omega \\
\tilde T\\
\tilde v\\
\tilde b_g\\
\tilde b_a\\
\tilde \omega_g
\end{bmatrix}
+
\begin{bmatrix}
-I_{3\times3} & 0_{3\times3}& 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & -\bar R & 0_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & I_{3\times3}& 0_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& I_{3\times3}\\
0_{3\times3} & 0_{3\times3} & 0_{3\times3}& 0_{3\times3}\\
\end{bmatrix}
\begin{bmatrix}
 n_g\\
 n_a\\
 n_{b_g}\\
 n_{b_a}\\
\end{bmatrix}
\end{equation}

For convenience, define $\tilde X \doteq [\tilde \omega, \tilde T, \tilde v, \tilde b_g, \tilde b_a, \tilde\omega_g]^\top \in \real^{18}$ and equation above reads
\begin{equation}
\dot{\tilde X} = F \tilde X + G n_{imu}
\end{equation}

\subsection{Online calibration of IMU intrinsics}
IMU intrinsics (scaling and misalignment factors) are typically obtained via an offline calibration procedure. Nevertheless, we can also treat them as unknown constants and calibrate them online by incorporating them into the state with small initial covariance.

Let $C_a\in\real^3$ and $C_g\in\real^3$ be the calibration matrix of the accelerometer and gyroscope respectively.~\footnote{To be precise, $C_a$ and $C_g$ live on some manifolds since they have some intrinsic structures. For instance, $C_a$ is composed of a scaling matrix which has positive entries on the diagonal and zeros otherwise, and an orthonormal misalignment matrix.}

\subsubsection{Rotation}
Recall the last line of Eq.~\eqref{eq-rotation-derivation}
$$
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}} \approx \bar R\asym{\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\omega_m-\bar b_g}
$$
To incorporate the gyro calibration matrix $C_g$, substitute the term $\omega_m$ with $C_g\omega_m$:
$$
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}} \approx \bar R\asym{C_g\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{C_g\omega_m-\bar b_g},
$$
and write the gyro calibration in terms of its nominal and error state $C_g=\bar C_g + \tilde C_g$:
\begin{align}
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
  &\approx \bar R\asym{(\bar C_g + \tilde C_g)\omega_m-\bar b_g} + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{(\bar C_g + \tilde C_g)\omega_m-\bar b_g}\\
 \dot{\bar R} + \dot{\bar R} \asym{\tilde \omega} + \bar R \asym{\dot{\tilde\omega}}
  &\approx
  \bar R\asym{\bar C_g\omega_m-\bar b_g}
  + \bar R\asym{\tilde C_g\omega_m} \\
  & \quad + \bar R\asym{-\tilde b_g - n_g}  \\
  & \quad + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}
  + \bar R \asym{\tilde \omega}\asym{\tilde C_g\omega_m} \text{ (2nd term is high-order)}\\
  &\approx
  \bar R\asym{\bar C_g\omega_m-\bar b_g} + \bar R\asym{\tilde C_g\omega_m}
  + \bar R\asym{-\tilde b_g - n_g} + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}
\end{align}

Now, the nominal state evolves as
$$
\dot{\bar R} = \bar R\asym{\bar C_g \omega_m - \bar b_g}
$$
resulting the identity $\bar R^\top \dot{\bar R} = \asym{\bar C_g \omega_m - \bar b_g}$ which can be used to simplify the rest of the equation:
$$
\dot{\bar R}\asym{\tilde\omega} + \bar R\asym{\dot{\tilde\omega}} =
  \bar R\asym{\tilde C_g\omega_m} + \bar R\asym{-\tilde b_g - n_g}
    + \bar R \asym{\tilde \omega}\asym{\bar C_g\omega_m-\bar b_g}.
$$
Let $\bar\omega=\bar C_g \omega_m - \bar b_g$, and left-multiple $\bar R^\top$ on both sides of the equation above to cancel out $\bar R$
\begin{align}
\asym{\bar\omega}\asym{\tilde\omega} + \asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}
    + \asym{\tilde \omega}\asym{\bar\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}
    + \big(\asym{\tilde\omega}\asym{\bar\omega}-\asym{\bar\omega}\asym{\tilde\omega}\big) \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}
    + \asym{\tilde\omega\times\bar\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}
    - \asym{\bar\omega\times\tilde\omega} \\
\asym{\dot{\tilde\omega}} &=
  \asym{\tilde C_g\omega_m} + \asym{-\tilde b_g - n_g}
    - \asym{\asym{\bar\omega}\tilde\omega} \\
\dot{\tilde\omega} &= \tilde C_g\omega_m - \tilde b_g - n_g - \asym{\bar\omega}\tilde\omega
\end{align}
leading to the error state equation of $\tilde\omega$:
$$
\dot{\tilde\omega} = -\asym{\bar\omega}\tilde{\omega} + \tilde{C_g} \omega_m - \tilde b_g - n_g
$$

\subsubsection{Velocity}

Recall the last line of Eq.~\eqref{eq-velocity-derivation}
$$
 \dot{\bar v} + \dot{\tilde v} \approx \bar R (a_m-\bar b_a) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}(a_m-\bar b_a) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g.
$$

To consider the accelerometer calibration, substitute $\alpha_m$ with $C_a\alpha_m=(\bar C_a+\tilde C_a)\alpha_m$:
$$
 \dot{\bar v} + \dot{\tilde v} \approx \bar R \big((\bar C_a + \tilde C_a)a_m-\bar b_a\big) -\bar R (\tilde b_a + n_a) + \bar R \asym{\tilde\omega}\big((\bar C_a + \tilde C_a) a_m-\bar b_a\big) + \bar R_g \gamma - \bar R_g \asym{\gamma}\tilde\omega_g.
$$

Nominal state evolves as $\dot{\bar v} = \bar R\bar\alpha + \bar R_g \gamma$, where $\bar\alpha\doteq\bar C_a \alpha_m - \bar b_a$.

The rest of the equation can be further simplified
\begin{align}
\dot{\tilde v} &= \bar R \tilde C_a a_m - \bar R(\tilde b_a + n_a) + \bar R\asym{\tilde\omega}\big(\bar\alpha  + \tilde C_a \alpha_m\big) - \bar R_g\asym{\gamma}\tilde\omega_g \\
\dot{\tilde v} &= \bar R \tilde C_a a_m - \bar R(\tilde b_a + n_a) - \bar R\asym{\bar\alpha}\tilde\omega - \bar R_g \asym{\gamma}\tilde\omega_g
\end{align}
by dropping 2nd-order residual terms and using the identity $\asym{a}b=-\asym{b}a$.

\subsection{Integration of motion}

With coming inertial measurements, the motion of the sensor platform can be obtained by numerical integration of Eq.~\eqref{eq:nominal-dynamics} (there are numerous textbooks on this topic, for instance, \cite{ascher1998computer}). As pointed out in \cite{ascher1998computer}, Fehlberg's, and Prince-Dormand's embedded Runge-Kutta methods are among the most popular ones. Some reference implementations can be found at \url{http://www.mymathlib.com/diffeq/embedded_runge_kutta/}.

To propagate the covariance matrix of the error state, we follow \cite{mourikis2007multi} where the covariance matrix is partitioned as follows:

\begin{equation}
    P_{k|k} =
    \begin{bmatrix}
        P_{k|k}^{(1)} & P_{k|k}^{(2)} \\
        P_{k|k}^{(2)\top} & P_{k|k}^{(3)}
    \end{bmatrix}
    \label{eq:covariance-partition}
\end{equation}
where $P_{k|k}^{(1)}$ is the $18\times 18$ covariance matrix of the evolving motion state, $P_{k|k}^{(3)}$ corresponds to the structure state (including pose of groups and position of features, detailed later), and $P_{k|k}^{(2)}$  is the correlation between the errors in the motion state and the structure state. The covariance matrix of the propagated state is given by:

\begin{equation}
    P_{k+1|k} =
    \begin{bmatrix}
        P_{k+1|k}^{(1)} & \Phi(t_{k + 1}, t_k) P_{k|k}^{(2)} \\
        P_{k|k}^{(2)\top}\Phi(t_{k + 1}, t_k)^\top & P_{k|k}^{(3)}  \\
    \end{bmatrix}
    \label{eq:covariance-propagation}
\end{equation}

where $P_{k+1|k}^{(1)}$ is computed by numerical integration of the following Lyapunov equation for the time interval $(t_k, t_{k+1})$ with initial condition $P=P_{k|k}^{(1)}$:

\begin{equation}
    \dot{P} = F P + P F^\top + G Q_{imu} G^\top.
\label{eq:lyapunov}
\end{equation}

The state transition matrix $\Phi(t_{k+1}, t_k)$ is obtained by numerical integration of the following matrix differential equation over $(t_k, t_{k+1})$ with initial condition $\Phi(t_k, t_k)=I_{18\times 18}$:

\begin{equation}
    \dot{\Phi}(\tau, t_k) = F \Phi(\tau, t_k), \quad\tau \in [t_k, t_{k+1}].
\end{equation}

\section{State augmentation}
Once the pose estimate of a newly arrived image is available, its camera-to-spatial transformation $g_{sc} \in \SE{3}$ is inserted into the state. $g_{sc}$ is the composed of the body-to-spatial transformation $g_{sb} \in \SE{3}$ and the camera-to-body alignment $g_{bc} \in \SE{3}$. Written in components
\begin{equation}
\begin{cases}
R_{sc} &= R_{sb} R_{bc}\\
T_{sc} &= R_{sb} T_{bc} + T_{sb}
\end{cases}
\end{equation}

\subsection{Rotation}
With error state representation, the rotation part is:
\begin{equation}
 \begin{aligned}
R_{sc} &= R_{sb} R_{bc} \\
 (\bar R_{sc} + \bar R_{sc} \asym{\tilde\omega_{sc}}) &=
 (\bar R_{sb} + \bar R_{sb} \asym{\tilde\omega_{sb}}) (\bar R_{bc} + \bar R_{bc} \asym{\tilde\omega_{bc}}) \\
 \bar R_{sc} + \bar R_{sc}\asym{\tilde \omega_{sc}} &\approx \bar R_{sb} \bar R_{bc}
 + \bar R_{sb} \asym{\tilde \omega_{sb}} \bar R_{bc} + \bar R_{sb} \bar R_{bc} \asym{\tilde\omega_{bc}} \quad \text{ drop } \asym{\tilde\omega_{sb}}\asym{\tilde\omega_{bc}}
 \end{aligned}
\end{equation}

Group terms on both sides into nominal part and error-state part respectively:
\begin{equation}
\begin{cases}
\bar R_{sc} &= \bar R_{sb} \bar R_{bc}\\
\asym{\tilde \omega_{sc}} &= \bar R_{sc}^\top \bar R_{sb} \asym{\tilde\omega_{sb}} \bar R_{bc} + \bar R_{sc}^\top \bar R_{sb} \bar R_{bc} \asym{\tilde\omega_{bc}}
\end{cases}
\end{equation}
where the second equation can be further simplified as $\tilde \omega_{sc} = \bar R_{bc}^\top \tilde \omega_{sb} + \tilde \omega_{bc}$ by noticing the following:
\begin{itemize}
 \item
$\bar R_{sc}^\top \bar R_{sb}=\bar R_{cb} = \bar R_{bc}^\top = \bar R_{bc}^{-1}$
\item
$R \asym{\omega} R^\top=\asym{R \omega} \quad \forall R \in \SO{3}, \omega \in \real^3$ (see appendix for a proof)
\end{itemize}

\subsection{Translation}
\begin{equation}
\begin{aligned}
T_{sc} &= R_{sb} T_{bc} + T_{sb} \\
\bar T_{sc} + \tilde T_{sc} &= (\bar R_{sb} + \bar R_{sb} \asym{\tilde \omega_{sb}}) (\bar T_{bc} + \tilde T_{bc}) + \bar T_{sb} + \tilde T_{sb} \\
\bar T_{sc} + \tilde T_{sc} &\approx \big(\bar R_{sb} \bar T_{bc} + \bar T_{sb} \big)
+ \bar R_{sb} \tilde T_{bc} + \bar R_{sb} \asym{\tilde \omega_{sb}} \bar T_{bc} + \tilde T_{sb} \quad \text{ drop } \bar R_{sb} \asym{\tilde \omega_{sb}} \tilde  T_{bc}
\end{aligned}
\end{equation}

Group terms into nominal and error-state equation and simplify:
\begin{equation}
\begin{cases}
 \bar T_{sc} &= \bar R_{sb} \bar T_{bc} + \bar T_{sb} \\
 \tilde T_{sc} &= \bar R_{sb} \tilde T_{bc} - \bar R_{sb} \asym{\bar T_{bc}} \tilde \omega_{sb} + \tilde T_{sb}
\end{cases}
\end{equation}

To summarize, the nominal equations for the augmented pose are
\begin{equation}
\begin{cases}
\bar R_{sc} &= \bar R_{sb} \bar R_{bc} \\
 \bar T_{sc} &= \bar R_{sb} \bar T_{bc} + \bar T_{sb}
\end{cases}
\end{equation}

and the error-state equations are
\begin{equation}
\begin{cases}
\tilde \omega_{sc} &= \bar R_{bc}^\top \tilde \omega_{sb} + \tilde \omega_{bc} \\
 \tilde T_{sc} &= \bar R_{sb} \tilde T_{bc} - \bar R_{sb} \asym{\bar T_{bc}} \tilde \omega_{sb} + \tilde T_{sb}
\end{cases}
\end{equation}

To augment the covariance matrix, build the Jacobian according to the error-state equation as $J \in \real^{6\times N}$ where the covariance matrix before augmentation is $P \in \real^{N\times N}$ then

\begin{equation}
 P \leftarrow
 \begin{bmatrix}
  I_{N\times N}\\
  J
 \end{bmatrix}
 P
 \begin{bmatrix}
 I_{N\times N}\\
 J
 \end{bmatrix}^\top
\end{equation}

Note, if camera-to-body transformation is not being estimated, simply set its error state  to zero, i.e., $\tilde \omega_{bc}=0$ and $\tilde T_{bc}=0$ and the nominal state as the true state (calibrated offline), i.e., $\bar R_{bc}=R_{bc}$ and $\bar T_{bc}=T_{bc}$.

\subsection{State augmentation with $g_{sb}$}

Instead of $g_{sc}$, we can augment the state with body-to-spatial transformation $g_{sb}$ of the arriving image and share the camera-to-body alignement $g_{bc}$  among them. In this case, initialization of the error state is trivial: Simply duplicate the nominal state, the error state and the covariance block corresponding to $g_{sb}$ of the current state being estimated

\subsection{State augmentation with 3D points}

Simply duplicate the state and covariance of the depth subfilter (Sect.~\ref{sect-depth}) into the main state and covariance.

\section{Measurement update - MSCKF}
Assume the depth subfilter (Sect.~\ref{sect-depth}) estimates the feature state well whose coordinates in the spatial frame $X_s\in\real^3$ can be computed. For each group $g_{sc}(t)$ to which the feature is visible, the pixel coordinates read

\begin{equation}
 x_p(t) = \pi(g_{sc}^{-1}(t)X_s) = \pi\big( R_{sc}^\top (X_s-T_{sc}) \big).
\end{equation}
Furthermore, if the state is augmented with camera-to-body pose $g_{sb}$ with shared camera-to-body alignemnt $ig_{bc}$ the visual measurements read

\begin{equation}
x_p(t) = \pi(g_{bc}^{-1}(t)g_{sb}^{-1}(t)X_s)=\pi\big(R_{bc}^\top (R_{sb}^\top(X_s-T_{sb}) - T_{bc})\big).
\end{equation}

In this section, we derive the error-state form of the measurement equations w/ and w/o the camera-to-body alignment incorporated into the augmented pose.  With the $g_{bc}$ incorporated into $g_{sc}$, we are assuming $g_{bc}$ is perfectly known after calibration before running the estimator, which is not realistic due to imperfection of the calibration procedure. With only $g_{sb}$ augmented to the state and the $g_{bc}$ shared among all the augmented poses, we will be able to perform online spatial calibration with the initial nominal value provided by the offline calibration.

\subsection{State augmentation with $g_{sc}$}

Taylor-expand the projection function
\begin{equation}
 \pi(\bar a + \tilde a)
 = \pi(\bar a) + \frac{d \pi}{d a}|_{a=\bar a} \tilde a + o(\tilde a)
 \approx \pi(\bar a) + \pi'(\bar a) \tilde a.
\end{equation}

Therefore if we write $R_{sc}^\top(X_s-T_{sc})$ in the form of $\bar a+\tilde a$, we will the following nominal and error-state equations:

\begin{equation}
\begin{cases}
\bar x_p = \pi(\bar a)\\
\tilde x_p = \pi'(\bar a) \tilde a
\end{cases}
\end{equation}

\begin{equation}
\begin{aligned}
R_{sc}^\top(X_s-T_{sc})
&= (\bar R_{sc} + \bar R_{sc}\asym{\tilde \omega_{sc}})^\top (\bar X_s - \bar T_{sc} + (\tilde X_s - \tilde T_{sc})) \\
R_{sc}^\top(X_s-T_{sc})
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
&= (\bar R_{sc}^\top - \asym{\tilde \omega_{sc}}\bar R_{sc}^\top) (\bar X_s - \bar T_{sc} + (\tilde X_s - \tilde T_{sc})) \\
R_{sc}^\top(X_s-T_{sc})
&\approx \bar R_{sc}^\top(\bar X_s - \bar T_{sc})
+ \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc})
- \asym{\tilde \omega_{sc}} \bar R_{sc}^\top(\bar X_s - \bar T_{sc})\\
R_{sc}^\top(X_s-T_{sc})
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
&\approx \bar R_{sc}^\top(\bar X_s - \bar T_{sc})
+ \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc})
+ \asym{\bar R_{sc}^\top(\bar X_s - \bar T_{sc})} \tilde\omega_{sc}\\
\end{aligned}
\end{equation}

To summarize:

\begin{equation}
\begin{cases}
\bar x_p=\pi\big( \bar R_{sc}^\top (\bar X_s - \bar T_{sc})\big)\\
\tilde x_p=\pi'\big(\bar R_{sc}^\top(\bar X_s - \bar T_{sc})\big)
\big( \bar R_{sc}^\top(\tilde X_s - \tilde T_{sc}) + \asym{\bar R_{sc}^\top(\bar X_s - \bar T_{sc})} \tilde \omega_{sc}\big)
\end{cases}
\end{equation}

\subsection{State augmentation with $g_{sb}$}
\label{sect-msckf-gsb}
Again, we need to write $R_{bc}^\top\big(R_{sb}^\top(X_s-T_{sb})-T_{bc})\big)$ in the form of $\bar a + \tilde a$:

\begin{equation}
\begin{aligned}
 & R_{bc}^\top\big(R_{sb}^\top(X_s-T_{sb})-T_{bc})\big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & (\bar R_{bc} + \bar R_{bc}\asym{\tilde\omega_{bc}})^\top
 \big( (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\bar X_s - \bar T_{sb}) + (\bar R_{sb} + \bar R_{sb}\asym{\tilde\omega_{sb}})^\top (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
 \big( (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) + (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) - (\bar T_{bc} + \tilde T_{bc}) \big) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
= & (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)
 (\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\bar X_s - \bar T_{sb}) \\
 &+ (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^
 \top)(\bar R_{sb}^\top - \asym{\tilde\omega_{sb}} \bar R_{sb}^\top) (\tilde X_s - \tilde T_{sb}) \\
 &- (\bar R_{bc}^\top - \asym{\tilde\omega_{bc}} \bar R_{bc}^\top)(\bar T_{bc} + \tilde T_{bc}) \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 \approx & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \asym{\tilde \omega_{bc}}\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \asym{\tilde \omega_{sb}} \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) \\
 &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
 &- \bar R_{bc}^\top \bar T_{bc} - \bar R_{bc}^\top \tilde T_{bc} + \asym{\tilde \omega_{bc}} \bar T_{bc} \quad\text{(drop high-order terms)}\\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 = & \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})
 + \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc}
 + \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})} \tilde \omega_{sb} \\
 &+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb}) \\
 &- \bar R_{bc}^\top \bar T_{bc}
 - \bar R_{bc}^\top \tilde T_{bc}
 - \asym{\bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc}  \\
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
=& \bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc} \quad\text{(nominal term)}\\
&+ \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{bc}
+ \bar R_{bc}^\top \asym{ \bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
 + \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb})
 - \bar R_{bc}^\top \tilde T_{bc} - \asym{\bar R_{bc}^\top \bar T_{bc}} \tilde \omega_{bc} \\
\end{aligned}
\end{equation}

To summarize:

\begin{equation}
\begin{cases}
\bar x_p &= \pi\big(\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}\big) \\
\tilde x_p &= \pi'\big(\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}\big) \\
    &\cdot

    \big(
    \asym{\bar R_{bc}^\top \bar R_{sb}^\top (\bar X_s - \bar T_{sb}) - \bar R_{bc}^\top \bar T_{bc}}\tilde \omega_{bc}
+ \bar R_{bc}^\top \asym{\bar R_{sb}^\top (\bar X_s - \bar T_{sb})}\tilde \omega_{sb}
+ \bar R_{bc}^\top\bar R_{sb}^\top (\tilde X_s - \tilde T_{sb})
- \bar R_{bc}^\top \tilde T_{bc}
    \big)
\end{cases}
\label{eq-msckf-gsb}
\end{equation}

\subsection{Online temporal calibration}

With the temporal offset $t_d$ in image timestamps, we need to bring the 3D point to time instant $t+t_d$ when the visual features are actually measured instead of time $t$ when the measurements are timestamped. The transformation reads $X_c = R_{bc}^\top\big(R_{sb}(t+t_d)^\top (X_s - T_{sb}(t+t_d)) - T_{bc}\big)$, where $T_{sb}\approx \bar T_{sb} + \bar v_{sb} \tilde t_d$,(2nd-order error terms dropped) and
$$
R_{sb}(t+t_d) \approx \bar R_{sb}(I+\asym{\tilde \omega_{sb}})(I + \asym{\bar\omega}\tilde t_d)=\bar R_{sb} + \bar R_{sb}\asym{\tilde \omega_{sb}} + \bar R_{sb}\asym{\bar\omega} \tilde t_d \quad\text{(drop 2nd-order error terms)}
$$
where $\bar R_{sb}\doteq \bar R_{sb}(t+\bar t_d)$ is the rotation propagated to time $(t+t_d)$, and $\bar \omega$ is the measured angular velocity.

Error terms involving $\tilde t_d$ in $X_c$ then reads
$$
\bar R_{bc}^\top \big(\bar R_{sb}\asym{\bar\omega}\tilde t_d)^\top X_s - \bar R_{bc}^\top \bar R_{sb}^\top \bar v_{sb} \tilde t_d
= -\bar R_{bc}^\top \big(\asym{\bar\omega}\bar R_{sb}^\top X_s + \bar R_{sb}^\top \bar v_{sb} \big)\tilde t_d.
$$

This residual block can be inserted to the second part of the measurement residual equation $\tilde x_p$ of Eq.~\eqref{eq-msckf-gsb}, i.e.,
$$
\tilde x_p=\pi'(\ldots)\cdot\big(\ldots
- \bar R_{bc}^\top \big(\asym{\bar\omega}\bar R_{sb}^\top X_s + \bar R_{sb}^\top \bar v_{sb} \big)\tilde t_d
\big)
$$

Note, if online IMU intrinsics calibration is enabled, $\bar \omega=C_g \omega_m + b_g$, where $C_g = \bar C_g + \tilde C_g$ and $b_g=\bar b_g + \tilde b_g$, and hence we need to expand $\tilde x_p$ w.r.t. the error state of both the IMU intrinsics $\tilde C_g$ and the bias $\tilde b_g$:

$$
\tilde x_p
= \pi'(\ldots)\cdot
\big(
    \ldots
    +\bar R_{bc}^\top\asym{\bar R_{sb}^\top X_s} \bar t_d
    ( \tilde C_g \omega_m + \tilde b_g)
\big)
$$

\section{Measurement update - EKF}

The derivation of the EKF measurement equation in error state form closely follows Sect.~\ref{sect-msckf-gsb}. In fact, we can write down the error state representation of $X_s\approx \bar{X}_s + \tilde{X}_s$ in terms of the feature parametrization (since the feature is in state now) and substitute each term into Eq.\eqref{eq-msckf-gsb}.

\subsection{Error state form of $X_s$}

Let's first express $X_s$ in error state form. Each 3D point is parametrized as $(x_c, y_c, \rho)$ where $\rho$ is the inverse depth, i.e., the coordinates of the 3D point represented in the its reference camera frame is $X_c = [x_c/\rho, y_c/\rho, 1/\rho]^\top$.

\begin{equation}
\begin{aligned}
X_s &= g_{sb}(t_r) g_{bc} X_c \quad\text{(let } g_{sb}(t_r)=[R_r|T_r] \text{)}\\
&= R_r(R_{bc} X_c + T_{bc}) + T_r \\
%&= R_r(R_{bc} (\bar X_c + \tilde X_c)
%+ T_{bc}) + T_r \quad\text{formally of } X_c = \bar X_c + \tilde X_c\\
&= (\bar R_r + \bar R_r \asym{\tilde \omega_r})(\bar R_{bc} + \bar R_{bc}\asym{\tilde \omega_{bc}})(\bar X_c + \tilde X_c) + (\bar R_r + \bar R_r \asym{\tilde \omega_r})(\bar T_{bc} + \tilde T_{bc}) + (\bar T_r + \tilde T_r) \\
&= \bar R_r \bar R_{bc} \bar X_c  +
\bar R_r \bar R_{bc} \tilde X_c +
\bar R_r \bar R_{bc} \asym{\tilde\omega_{bc}} \bar X_c +
\bar R_r \asym{\tilde \omega_r} \bar R_{bc} \bar X_c \quad\text{(1st term, drop higher-order terms)}\\
&\quad + \bar R_r \bar T_{bc} + \bar R_r \tilde T_{bc} + \bar R_r \asym{\tilde\omega_r} \bar T_{bc} \quad\text{(2nd term, drop higher-order terms)}\\
&\quad + \bar T_r + \tilde T_r.
\end{aligned}
\end{equation}

To summarize:
\begin{equation}
\begin{cases}
\bar X_s &= \bar R_r \bar R_{bc} \bar X_c + \bar R_r \bar T_{bc} + \bar T_r\\
\tilde X_s &= \bar R_r \bar R_{bc} \tilde X_c - \bar R_r \bar R_{bc} \asym{\bar X_c} \tilde \omega_{bc} - \bar R_r \asym{\bar R_{bc} \bar X_c + \bar T_{bc}} \tilde \omega_r + \bar R_r \tilde T_{bc} + \tilde T_r.
\end{cases}
\end{equation}

Now we need to expand $X_c = [x_c/\rho, y_c/\rho, 1/\rho]^\top$ in error state form. Let's start with x-component (same procedure applies to y-component due to symmetry):
\begin{equation}
\begin{aligned}
\frac{x_c}{\rho}
&= \frac{\bar x_c + \tilde x_c}{\bar \rho_c + \tilde \rho_c} \\
&= (\bar x_c + \tilde x_c) \left (  \frac{\bar \rho_c - \tilde \rho_c}{\bar \rho_c^2 - \tilde \rho_c^2}  \right ) \\
&\approx (\bar x_c + \tilde x_c) \left (  \frac{\bar \rho_c - \tilde \rho_c}{\bar \rho_c^2}  \right ) \quad\text{(drop higher-order term in denominator)}\\
&= (\bar x_c + \tilde x_c)\left (\frac{1}{\bar \rho_c} - \frac{1}{\bar \rho_c^2}\tilde \rho_c \right )\\
&\approx \frac{\bar x_c}{\bar\rho_c} - \frac{\bar x_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde x_c \quad\text{(drop higher-order terms)}.
\end{aligned}
\end{equation}

\begin{equation}
\begin{cases}
\bar X_c &=
\frac{1}{\bar \rho_c}
\begin{bmatrix}
    \bar x_c\\
    \bar y_c\\
    1
\end{bmatrix} \\
\tilde X_c &=
\begin{bmatrix}
-\frac{\bar x_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde x_c \\
-\frac{\bar y_c}{\bar \rho_c^2} \tilde\rho_c + \frac{1}{\bar \rho_c} \tilde y_c \\
-\frac{1}{\bar \rho_c^2} \tilde \rho_c
\end{bmatrix}
=
\begin{bmatrix}
    1/\bar\rho_c & 0 & -\bar x_c / \bar\rho_c^2\\
    0 & 1/\bar\rho_c & -\bar y_c / \bar\rho_c^2\\
    0 & 0 & -1 / \bar\rho_c^2\\
\end{bmatrix}
\begin{bmatrix}
    \tilde x_c \\
    \tilde y_c \\
    \tilde \rho_c
\end{bmatrix}
\end{cases}
\end{equation}
Substitute the above into $\bar X_s, \tilde X_s$, and we get the full measurement equation in error state form.

\section{Depth subfilter}
\label{sect-depth}
When a feature is first observed at time $t_r$ with pixel coordinates $[x_p(t_r) , y_p(t_r)]^\top\in\real^2$, we associate it with a reference group with camera pose $g_{ref}\doteq g_{sc}(t_r)$ and initialize its state as $x=[x_c, y_c, \rho]^\top \in \real^3$ where $\rho \in \real_+$ is the inverse depth initialized at a nominal distance to the image plane and $[x_c, y_c]^\top$ is the feature position in camera coordinates:
\begin{equation}
\begin{bmatrix}
x_c\\
y_c\\
1
\end{bmatrix}
= \pi^{-1}
\begin{bmatrix}
x_p\\
y_p\\
1
\end{bmatrix}
\end{equation}

In the depth subfilter, camera poses are treated as known and we estimate the state $x$ which has trivial dynamics $\dot x=0$ assuming the feature is static in space. When a new image arrives at time $t$ to which the same feature is observed at pixel location $x_p(t)\in\real^2$, the feature state is updated using the following measurement model:
\begin{equation}
 x_p(t) = \pi\big(g_{bc}^{-1}(t) g_{sb}^{-1}(t) g_{sc}(t_r)
 \frac{1}{\rho}
\begin{bmatrix}
x_c\\
y_c\\
1
\end{bmatrix}
\big)
\end{equation}

% \section{Feature process}

% When a feature is dropped by the tracker, update all the groups which see the feature.

% When a new image arrives, create a new group (the total number of groups might exceed the limitation, address later), attach the newly created features to the new group. However, since this group is just observed, no features can be used to update its pose. We should create the new group *after we process all the existing features* with the updated pose.

% Once a new group has been created, attach the newly created features to it and initialize the features at nominal distance to the image plane.

% After the measurement update, if there are groups which do not see any features, remove them from the state. Then add the new group to state.

% \section{Evaluation on TUM-VI dataset}

% We compare Corvis~\cite{tsotsosCS15} and our new implementation XIVO against
% \begin{itemize}
%     \item OKVIS of Leutenegger {\it et al.}~\cite{leutenegger2015keyframe}
%     \item ROVIO of Bloesch {\it et al.}~\cite{bloesch2015robust,bloesch2017iterated}
%     \item VINS of Qin {\it et al.}~\cite{qin2018vins}
% \end{itemize}
% which have been benchmarked on the TUM-VI dataset \cite{schubert2018tum}. Corvis, XIVO and ROVIO are EKF-based, whereas OKVIS and VINS perform keyframe-based nonlinear optimization.

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % EVALUATION WITH TUM RGB-D BENCHMARK SCRIPT
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% \begin{table}[h]
% \begin{center}
% \begin{tabular}{c|c|c|c||c|c|c}
%     \hline
%      \multicolumn{2}{c|}{} & \multicolumn{2}{c||}{Keyframe-based} & \multicolumn{3}{c}{EKF-based} \\
%     \hline
%     Sequence & length & OKVIS & VINS & ROVIO & Corvis & XIVO \\
%     \hline\hline
%     room1 & 156{\em m} & \textbf{0.06}{\em m} & 0.07{\em m} & 0.16{\em m}& 0.10{\em m} & 0.22{\em m} \\

%     room2 & 142{\em m} & 0.11{\em m} & \textbf{0.07}{\em m} & 0.33{\em m}& 0.15{\em m} & 0.08{\em m} \\

%     room3 & 135{\em m} & \textbf{0.07}{\em m} & 0.11{\em m} & 0.15{\em m}& 0.12{\em m} &0.11{\em m} \\

%     room4 & 68{\em m} & \textbf{0.03}{\em m} & 0.04{\em m}  & 0.09{\em m}& 0.12{\em m} & 0.08{\em m} \\

%     room5 & 131{\em m} & \textbf{0.07}{\em m} & 0.20{\em m} & 0.12{\em m}& 0.12{\em m} & 0.11{\em m} \\

%     room6 & 67{\em m} & \textbf{0.04}{\em m} & 0.08{\em m}  & 0.05{\em m}& 0.06{\em m} & 0.12{\em m} \\

%     \hline
% \end{tabular}
% \end{center}
% \caption{\textit{RMSE ATE} in meters.}
% \end{table}

% \begin{table}[h]
% \begin{center}
% \begin{tabular}{c|c|c||c|c|c}
%     \hline
%       & \multicolumn{2}{c||}{Keyframe-based} & \multicolumn{3}{c}{EKF-based} \\
%      \hline
%     Sequence & OKVIS & VINS & ROVIO & Corvis & XIVO \\
%     \hline\hline
%     room1 & \textbf{0.013}{\em m} / \textbf{0.43}$\degree$ & 0.015{\em m} / 0.44$\degree$ & 0.029{\em m} / 0.53$\degree$ & 0.047{\em m} / 3.45$\degree$ & 0.031{\em m} / 0.59$\degree$\\

%     room2 & \textbf{0.015}{\em m} / \textbf{0.62}$\degree$ & 0.017{\em m} / 0.63$\degree$ & 0.030{\em m} / 0.67$\degree$ & 0.037{\em m} / 2.56$\degree$ & 0.023{\em m} / 0.75$\degree$\\

%     room3 & \textbf{0.012}{\em m} / \textbf{0.63}$\degree$ & 0.023{\em m} / \textbf{0.63}$\degree$ & 0.027{\em m} / 0.66$\degree$ & 0.033{\em m} / 2.66$\degree$ & 0.027{\em m} / 0.73$\degree$\\

%     room4 & \textbf{0.012}{\em m} / 0.57$\degree$ & 0.015{\em m} / \textbf{0.41}$\degree$ & 0.022{\em m} / 0.61$\degree$ & 0.13{\em m} / 4.80$\degree$& 0.023{\em m} / 0.62$\degree$\\

%     room5 & \textbf{0.012}{\em m} / \textbf{0.47}$\degree$ & 0.026{\em m} / \textbf{0.47}$\degree$ & 0.031{\em m} / 0.60$\degree$ & 0.052{\em m} / 4.59$\degree$& 0.023{\em m} / 0.65$\degree$\\

%     room6 & \textbf{0.012}{\em m} / 0.49$\degree$ & 0.014{\em m} / \textbf{0.44}$\degree$ & 0.019{\em m} / 0.50$\degree$ & 0.020{\em m} / 2.04$\degree$& 0.031{\em m} / 0.53$\degree$\\
%     \hline
% \end{tabular}
% \end{center}
% \caption{\textit{RMSE RPE} on 1 second segments.}
% \end{table}

% \section{Planned features}

% \begin{enumerate}
%     \item Online camera intrinsics calibration.
%     \item Loop closure.
%     \item Hybrid in-state and out-of-state (MSCKF) measurement update.
% \end{enumerate}

\clearpage

\appendix
\section{Skew-symmetric matrix identities}

\begin{prop}
If $R \in\SO{3}, \omega\in\real^3$, then $R\asym{\omega} R^\top=\asym{R\omega}$.
\end{prop}

\begin{proof}
Let $\SO{3} \ni R^\top=[r_1, r_2, r_3]$, then $r_i^\top r_j=\delta_{ij}$ and

\begin{equation}
\begin{aligned}
r_1 \times r_2=r_3\\
r_2 \times r_3=r_1\\
r_3 \times r_1=r_2\\
\end{aligned}
\label{eq-basis}
\end{equation}

recall $\asym{a} b\doteq a\times b$ and $a\times b = -b\times a$.

\begin{equation}
\begin{aligned}
R\asym{\omega}R^\top
&=
\begin{bmatrix}
r_1^\top\\
r_2^\top\\
r_3^\top
\end{bmatrix}
\asym{\omega}
\begin{bmatrix}
r_1 & r_2 & r_3
\end{bmatrix}\\
&=
\begin{bmatrix}
r_1^\top \asym{\omega}r_1 & r_1^\top \asym{\omega}r_2 & r_1^\top\asym{\omega} r_3 \\
r_2^\top \asym{\omega}r_1 & r_2^\top \asym{\omega}r_2 & r_2^\top\asym{\omega} r_3 \\
r_3^\top \asym{\omega}r_1 & r_3^\top \asym{\omega}r_2 & r_3^\top\asym{\omega} r_3 \\
\end{bmatrix}
\end{aligned}
\end{equation}

Notice $r_i^\top\asym\omega r_i=r_i^\top (\omega \times r_i)=0$ and $\real\ni r_i^\top \asym\omega r_j=\big( r_i^\top \asym\omega r_j \big)^\top=r_j^\top (\asym\omega)^\top r_i=-r_j^\top\asym\omega r_i$ since $\asym\cdot$ is skew-symmetric. Therefore  $R\asym\omega R^\top \in so(3)$ to which we can apply the \textit{vee} operator to bring it to $\real^3$
\begin{equation}
\big(R\asym{\omega}R^\top\big)^\vee
=
\begin{bmatrix}
r_3^\top\asym\omega r_2\\
r_1^\top\asym\omega r_3\\
r_2^\top\asym\omega r_1
\end{bmatrix}
\end{equation}

Now we only need to show $\big(R\asym\omega R^\top\big)^\vee=R\omega=[r_1^\top \omega, r_2^\top \omega, r_3^\top \omega]^\top$. Without loss of generality, let's show the equality holds for the first component, \textit{i.e.},
\begin{equation}
\begin{aligned}
& r_3^\top\asym\omega r_2 = r_1^\top\omega \quad \forall \omega \in \real^3\\
\Leftrightarrow  & -r_3^\top \asym{r_2} \omega = r_1^\top \omega \quad \forall \omega \in \real^3 \\
\Leftrightarrow & -r_3^\top \asym{r_2} = r_1^\top \\
\Leftrightarrow & \asym{r_2} r_3 = r_1 \quad \text{ skew-symmetric } \asym\cdot^\top=-\asym\cdot\\
\Leftrightarrow & r_2 \times r_3 = r_1
\end{aligned}
\end{equation}
where last equation holds due to properties of rotation matrics Eq.~\eqref{eq-basis}.
\end{proof}

\begin{prop}
If $a, b\in\real^3$, then $\big(\asym{a}\asym{b}\big)^\top=\asym{b}\asym{a}$.
\end{prop}

\begin{proof}
$\big(\asym{a}\asym{b}\big)^\top=\asym{b}^\top\asym{a}^\top=-\asym{b}\cdot -\asym{a}=\asym{b}\asym{a}$
\end{proof}

\begin{prop}
If $a, b\in\real^3$, then $\asym{(a\times b)} = \asym{a}\asym{b} - \asym{b}\asym{a}$.
\end{prop}
\begin{proof}
    Expand both sides as $3\times 3$ matrices and verify each entry.
\end{proof}

\section{Some transformations and their Jacobians}

Some useful transformation and the Jacobians with respect to the error state of each part involved in the transformation.

\subsection{Notation}
\begin{itemize}
    \item Rigid body transformation: $g=[R|T]\in\SE{3}, \text{ where } R\in\SO{3}, T\in\real^3$. Error state representation: $R=\bar R(I+\asym{\omega})$, $T=\bar T + \tilde T$
    \item 3D point: $X=\bar X + \tilde X \in \real^3$
\end{itemize}

\subsection{Apply rigid body transformation to a 3D point}

The new point $X_n=gX$, it is easy to see the nominal state and error state representation of $X_n$ is:

$$
\bar X_n = \bar R \bar X + \bar T, \text{ and } \tilde X_n = \bar R\tilde X - \bar R\asym{\bar X}\omega + \tilde T
$$

\subsection{Composition of rigid body transformation}

The composed rigid body motion $g\doteq [R|T] = g_1 g_2=[R_1 R_2|R_1 T_2 + T_1]$

To obtain the error state representation of the rotation, expand both sides of the rotational part of the equation:
\begin{align}
    \bar R (I+\asym{\omega}) &= \bar R_1(I+\asym{\omega_1})\bar R_2(I+\asym{\omega_2}) \\
    \bar R + \bar R\asym{\omega} &= \bar R_1 \bar R_2 + \bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(drop 2nd-order error term)}
\end{align}

It's easy to see the nominal state is $\bar R = \bar R_1 \bar R_2$, and the error state equation $\bar R\asym{\omega}=\bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}$ requires more work, since the error term $\omega$ is not expressed as a linear combination of $\omega_1, \omega_2$ yet:

\begin{align}
    \bar R \asym{\omega} &= \bar R_1\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2} \\
    \bar R \asym{\omega} &= \bar R_1 \bar R_2 \bar R_2^\top\asym{\omega_1}\bar R_2 + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(insert identity} I\doteq \bar R_2\bar R_2^\top \text{)}\\
    \bar R \asym{\omega} &= \bar R_1 \bar R_2 \asym{\bar R_2^\top\omega_1} + \bar R_1 \bar R_2 \asym{\omega_2}\quad\text{(use identity} R\asym{\omega}R^\top=\asym{R \omega} \text{)} \\
    \bar R \asym{\omega} &= \bar R \asym{\bar R_2^\top\omega_1} + \bar R\asym{\omega_2}\quad\text{(use nominal equation} \bar R=\bar R_1 \bar R_2 \text{)} \\
    \asym{\omega} &= \asym{\bar R_2^\top \omega_1} + \asym{\omega_2} \\
    \omega &= \bar R_2^\top \omega_1 + \omega_2\quad\text{(}\asym{\cdot}\text{ is linear)}
\end{align}

For the translation part:
\begin{align}
\bar T + \tilde T &= \bar R_1(I + \asym{\omega_1})(\bar T_2 + \tilde T_2) + \bar T_1 + \tilde T_1 \\
\bar T + \tilde T &= (\bar R_1 \bar T_2 + \bar T_1) + \bar R_1 \tilde T_2 + \bar R_1 \asym{\omega_1} \bar T_2 + \tilde T_1 \\
\bar T + \tilde T &= (\bar R_1 \bar T_2 + \bar T_1) + \bar R_1 \tilde T_2 - \bar R_1 \asym{\bar T_2} \omega_1 + \tilde T_1,
\end{align}
thus the nominal and error state equations are:
$$
\bar T = \bar R_1 \bar T_2 + \bar T_1 \quad\text{ and } \tilde T = \bar R_1 \tilde T_2 -\bar R_1 \asym{\bar T_2} \omega_1 + \tilde T_1
$$

\subsection{Inverse of rigid body transformation}
The inverse of rigid body transformation $g=[R | T]$ is $g^{-1}\doteq [R_i | T_i]=[R^\top|-R^\top T]$

Error state equation for the rotational part:
\begin{align}
    \bar R_i (I + \asym{\omega_i}) &= \big(\bar R(I + \asym{\omega})\big)^\top \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top + \asym{\omega}^\top \bar R^\top \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \asym{\omega} \bar R^\top\quad\text{(}\asym{\cdot}\text{ is asymmetric)}\\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \bar R^\top \bar R \asym{\omega} \bar R^\top\quad\text{(insert identity} I=\bar R^\top\bar R\text{ )} \\
    \bar R_i + \bar R_i \asym{\omega_i} &= \bar R^\top - \bar R^\top \asym{\bar R\omega}\quad\text{(use identity} R\asym{\omega}R^\top=\asym{R\omega},
\end{align}

Now the nominal equation is $\bar R_i = \bar R^\top$, and the error state equation is:
\begin{align}
    \bar R_i\asym{\omega_i} &= -\bar R^\top\asym{\bar R\omega} \\
    \bar R^\top \asym{\omega_i} &= -\bar R^\top\asym{\bar R\omega}\quad\text{(since} \bar R_i=\bar R^\top\text{ )} \\
    \asym{\omega_i} &= -\asym{\bar R\omega} \\
    \omega_i &= -\bar R\omega\quad\text{(} \asym{\cdot}\text{ is linear)}.
\end{align}

For the translational part:
\begin{align}
    \bar T_i + \tilde T_i &= -\big(\bar R(I + \asym{\omega})\big)^\top(\bar T + \tilde T) \\
    \bar T_i + \tilde T_i &= (-\bar R^\top + \asym{\omega}\bar R^\top)(\bar T + \tilde T) \\
    \bar T_i + \tilde T_i &= -\bar R^\top \bar T + \asym{\omega}\bar R^\top\bar T -\bar R^\top \tilde T \\
    \bar T_i + \tilde T_i &= -\bar R^\top \bar T - \asym{\bar R^\top\bar T}\omega -\bar R^\top \tilde T,
\end{align}
thus the nominal and error state equations are:
$$
\bar T_i = -\bar R^\top\bar T,\text{ and } \tilde T_i = -\asym{\bar R^\top\bar T}\omega - \bar R^\top \tilde T.
$$

\subsection{General composition rule}
Given a nonlinear function $g_2=f(g_1): \SE{3}\mapsto\SE{3}$, and assume its corresponding error state equation reads
$$
\omega_2 = A \omega_1, \text{ and } \tilde T_2 = B \omega_1 + C \tilde T_1.
$$

Now if we pass $g_2$ to another nonlinear function $g_3=h(g_2):\SE{3}\mapsto\SE{3}$ whose error state equation reads
$$
\omega_3 = D \omega_2, \text{ and } \tilde T_3 = E \omega_2 + F \tilde T_2,
$$

then the error state for $g_3=h(f(g_1)$ reads:
$$
\omega_3 = D A \omega_1, \text{ and } \tilde T_3 = E A \omega_1 + F(B\omega_1 + C\tilde T_1).
$$

\clearpage
\bibliographystyle{unsrt}
\bibliography{refs}

\end{document}


\chapter{Discrete-Time Stochastic Optimal Control}
\label{ch:discrete_time_stochastic_optimal_control}

In sequential decision problems, an agent interacts with an environment by
selecting a series of actions in order to complete a specific task. During this
interaction, the agent receives a numerical reward from the environment and its
goal is to find the best strategy in order to maximize a certain measure of its
performance. The environment evolves stochastically and may be influenced by
the interaction with the agent, so that each action taken by the agent may
influence the circumstances under which future decisions will be made.
Therefore, the agent must balance his desire to obtain a large reward today
by acting greedily and the opportunities that will be available in the future.
While this setting appears quite simple, it is general enough to encompasses a
wide range of applications in different fields. A classical example is
portfolio management, where an investor must allocate his capital so as to
maximize his long-term profits. Another standard example is chess, where two
players successively move pieces around the chessboard to checkmate the
opponent's king.\\
The purpose of the following sections is to introduce the notation that will be
used in the rest of this work and to recall the fundamental concepts and
results of the discrete-time stochastic optimal control theory, which is the
standard framework to study sequential decisions problems in mathematical
terms. Since our discussion will be far from being comprehensive, we refer the
reader to the extensive literature on the subject, such as
\cite{bertsekas1978stochastic}, \cite{puterman1994markov},
\cite{bertsekas1995dynamic}.

\section{Markov Decision Processes}
A sequential decision problem under uncertainty can be schematized as in Figure
\ref{fig:sequential_decision_problem}: at a given time $t$, the agent (also
known as decision maker or controller) observes the state $s_t$ of the system
(also know as environment) and subsequently performs an action $a_t$. Following
this action, the agent receives an immediate reward $r_{t+1}$ (or incurs an
immediate cost) and the system evolves to a new state according to a probability
distribution which depends on the action selected by the agent. At the
subsequent time $t+1$, the agent selects a new action given the new state of
the system and the process repeats. This interaction can be modeled rigorously
using a Markov decision process.
\begin{definition}[Markov Decision Process]
	A Markov decision process (MDP) is a stochastic dynamical system specified by the tuple $<\S, \A, \calP, \calR,
	\gamma>$, where
	\begin{enumerate}[label={\roman*)}]
		\item $(\S, \calS)$ is a measurable space, called the state space.
		\item $(\A, \calA)$ is a meausrable space, called the action space.
		\item $\calP: \S \times \A \times \calS \to \R$ is a Markov transition
			kernel, i.e.
			\begin{enumerate}[label={\alph*)}]
				\item for every $s\in\S$ and $a\in\A$, $B \mapsto \calP(s,a,B)$
					  is a probability distribution over $(\S, \calS)$.
				\item for every $B\in\calS$, $(s,a) \mapsto \calP(s,a,B)$ is
					  a measurable function on $\S \times \A$.
			\end{enumerate}
		\item $\calR: \S \times \A \to \R$ is a reward function.
		\item $\gamma \in (0,1)$ is a discount factor.
	\end{enumerate}
\end{definition}
Typically, the state space (and similarly the action space) will be either
finite, namely $\S = \{s_1, \ldots, s_d\}$, or continuous, namely $\S \subseteq
\R^{D_s}$. The kernel $\calP$ describes the random evolution of the system:
suppose that at time $t$ the system is in state $s$ and that the agent takes
action $a$, then, regardless of the previous history of the system, the
probability to find the system in a state belonging to $B\in\calS$ at time
$t+1$ is given by
\begin{equation}
	\calP(s, a, B) = \P{S_{t+1} \in B | S_t = s, A_t = a}
\end{equation}
Following this random transition, the agent receives a stochastic reward
$R_{t+1}$. The reward function $\calR(s, a)$ gives the expected reward
obtained when action $a$ is taken in state $s$
\begin{equation}
	\calR(s, a) = \E{R_{t+1} | S_t = s, A_t = a}
\end{equation}
This setting can be easily generalized to the following cases
\begin{enumerate}
	\item The initial state of the system is a random variable.
	\item The actions that an agent can select depend on the state of the system.
\end{enumerate}

\begin{figure}[t]
	\centering
	\begin{tikzpicture}[node distance = 6em, auto, thick]
		\node [block] (Agent) {Agent};
		\node [block, below of=Agent] (Environment) {Environment};
		\path [line] (Agent.0) --++ (4em,0em) |- node [near start]{Action $a_t$} (Environment.0);
		\path [line] (Environment.190) --++ (-6em,0em) |- node [near start]{State  $s_{t}$} (Agent.170);
		\path [line] (Environment.170) --++ (-4.25em,0em) |- node [near start, right] {Reward $r_{t+1}$} (Agent.190);
	\end{tikzpicture}
	\caption{Agent-environment interaction in sequential decision problems.}
	\label{fig:sequential_decision_problem}
\end{figure}

\section{Policies}
At any time step, the agent selects its actions according to a certain policy.
\begin{definition}[Policy]
	A policy is a function $\pi: \S \times \calA \to \R$ such that
	\begin{enumerate}[label={\roman*)}]
		\item for every $s \in \S$, $C \mapsto \pi(s,C)$ is a probability
			  distribution over $(\A, \calA)$.
		\item for every $C \in \calA$, $s \mapsto \pi(s, C)$ is a measurable
			  function.
	\end{enumerate}
\end{definition}
Intuitively, a policy represents a stochastic mapping from the current state of
the system to actions. Deterministic policies are a particular case of this
general definition. We assumed that the agent's policy is stationary and only
depends on the current state of the system. We might in fact  consider more
general policies that depends on the whole history of the system. However, as
we will see, we can always find an optimal policy that depends only on the
current state, so that our definition is not restrictive. A policy $\pi$ and an
initial state $s_0 \in \S$ determine a random state-action-reward sequence
${\{(S_t, A_t, R_{t+1})\}}_{t\geq 0}$ with values on $\S \times \A \times \R$
following the mechanism described above.
\begin{definition}[History]
	Given an initial state $s_0 \in \S$ and a policy $\pi$, a history (or
	equivalently trajectory or roll-out) of the system is a random sequence
	$H_\pi = {\{(S_t, A_t)\}}_{t\geq 0}$ with values in $\S \times \A$, defined
	on some probability space $(\Omega, \mathcal{F}, \mathbb{P})$, such that for
	$t = 0, 1, \ldots$
	\begin{equation*}
		\begin{cases}
			S_0 = s_0\\
			A_t \sim \pi(S_t, \cdot)\\
			S_{t+1} \sim \calP(S_t, A_t, \cdot)\\
		\end{cases}
	\end{equation*}
	we will denote by $(\H, \calH)$ the measurable space of all possible
	histories.
\end{definition}
Moreover, we observe that
\begin{enumerate}[label={\roman*)}]
	\item the state sequence ${\{S_t\}}_{t\geq 0}$ is a Markov process $<\S,
		  \calP_\pi>$.
	  \item the state-reward sequence ${\{(S_t, R_{t+1})\}}_{t\geq 0}$ is a Markov
		  reward process $<\S, \calP_\pi, \calR_\pi, \gamma>$.
\end{enumerate}
where we denoted
\begin{equation*}
	\begin{split}
		\calP_\pi(s, s') &= \int_\A \pi(s, a) \calP(s, a, s') da\\
		\calR_\pi(s) &= \int_\A \pi(s, a) \calR(s, a) da\\
	\end{split}
\end{equation*}
In stochastic optimal control, the goal of the agent is to find a policy that
maximizes a measure of the agent's long-term performance. In the next sections
we discuss some objective functions that are commonly used in the infinite
horizon framework.

\section{Risk-Neutral Framework}
In the risk-neutral setting, the agent is only interested in maximizing its reward, without considering the risk it needs to take on to achieve it. In an infinite horizon task, the agent's performance is typically measured either as the total discounted reward or as the average reward obtained at each time step. These two approaches are radically different both from a theoretical and an algorithmic point of view. For this reason, they will be always treated separately.

\subsection{Discounted Reward Formulation}
In the discounted reward formulation, the agent's performance is measured as
the expected return obtained following a specific policy.
\begin{definition}[Return]
	The return is the total discounted reward obtained by the agent starting
	from $t$
	\begin{equation*}
		G_t = \sum^{\infty}_{t=0} \gamma^t R_{t+k+1}
	\end{equation*}
	where $0 < \gamma < 1$ is the discount factor.
\end{definition}
In some domains, such as economics, discounting can be used to represent
interest earned on rewards, so that an action that generates an immediate
reward will be preferred over one that generates the same reward some steps
into the future. Discounting thus models the trade-off between immediate and
delayed reward: if $\gamma = 0$ the agent selects its actions in a myopic way,
while if $\gamma \to 1$ it acts in a far-sighted manner. There are other
possible reasons for discounting future rewards. The first is because it is
mathematically convenient, as it avoids infinite returns and it solves many
convergence issues. Another interpretation is that it models the uncertainty
about the future, which may not be fully represented. Indeed, the discount
factor could be seen as the probability that the world does not stop at a given
time step.
\begin{definition}[State-Value Function]
	The state-value function $V_\pi: \S \to \R$ is the expected return that can
	be obtained starting from a state and following policy $\pi$
	\begin{equation}
		V_\pi(s) = \E[\pi]{G_t|S_t = s}
	\end{equation}
\end{definition}
where the subscript in $\mathbb{E}_{\pi}$ indicates that all the actions are selected according to policy $\pi$. The state-value function measures how good it is for the agent to be in a given state and follow a certain policy. Similarly, we can introduce an action-value function that measures how good it is for the agent to be in a state, take a certain action and then follow the policy.
\begin{definition}[Action-Value Function]
	The action-value function $Q_\pi: \S \times \A \to \R$ is the expected
	return that can be obtained starting from a state, taking an action and
	then following policy $\pi$
	\begin{equation}
		Q_\pi(s,a) = \E[\pi]{G_t|S_t = s, A_t = a}
	\end{equation}
\end{definition}
We have the following relationship between $V_\pi$ and $Q_\pi$
\begin{equation}
	V_\pi(s) = \int_\A \pi(s,a) Q_\pi(s,a) da
\end{equation}
Almost all reinforcement learning algorithms are designed to estimate these
value functions and are typically based on the Bellman equations.
\begin{proposition}[Bellman Expectation Equations]
	\begin{equation}
		V_\pi(s) = \calR_\pi(s) + \gamma T_\pi V_\pi(s)
		\label{eq:bellman_expectation_eq_V}
	\end{equation}
	\begin{equation}
			Q_\pi(s,a) = \calR(s,a) + \gamma T_a V_\pi(s)
			\label{eq:bellman_expectation_eq_Q}
	\end{equation}
	where we denoted by $T_a$ (resp. $T_\pi$) the transition operator for action
	$a$ (resp. for policy $\pi$)
	\begin{equation*}
		\begin{split}
			T_a F(s) &= \E{F(S_{t+1})|S_t = s, A_t = a} = \int_\S \calP(s, a, s') F(s') ds'\\
			T_\pi F(s) &= \E[\pi]{F(S_{t+1})|S_t = s} = \int_\A \pi(s,a) \int_\S \calP(s,a,s') F(s') ds'
			da\\
		\end{split}
	\end{equation*}
\end{proposition}
If we introduce the Bellman expection operator $B_\pi$, defined as
\begin{equation*}
	B_\pi V_\pi(s) = \calR_\pi(s) + \gamma T_\pi V_\pi(s)
\end{equation*}
Then Eq. (\ref{eq:bellman_expectation_eq_V}) can be written as a fixed-point equation
\begin{equation*}
	V_\pi(s) = B_\pi V_\pi(s)
\end{equation*}
which, under some simple assumptions on the reward functions, admits a unique
solution by the contraction mapping theorem. A similar argument holds for Eq.
(\ref{eq:bellman_expectation_eq_Q}). The goal of the agent is to select a policy
$\pi_*$ that maximizes his expected return in all possible states. Such a
policy is called \emph{optimal}.
\begin{definition}[Optimal State-Value Function]
	The optimal state-value function $V_*: \S \to \R$ is the largest expected
	return that can be obtained starting from a state
	\begin{equation}
		V_*(s) = \sup_\pi V_\pi(s)
	\end{equation}
\end{definition}
\begin{definition}[Optimal Action-Value Function]
	The optimal action-value function $Q_*: \S \times \A \to \R$ is the largest
	expected return that can be obtained starting from a state and taking an
	action
	\begin{equation}
		Q_*(s,a) = \sup_\pi Q_\pi(s,a)
	\end{equation}
\end{definition}
The optimal value functions satisfy the following Bellman equations.
\begin{proposition}[Bellman Optimality Equations]
	\begin{equation}
		V_*(s) = \sup_a Q_*(s,a) = \sup_a \left\{\calR(s,a) + \gamma T_a V_*(s)\right\}
	\end{equation}
	\begin{equation}
		\begin{split}
			Q_*(s,a) &= \calR(s,a) + \gamma T_a V_*(s)\\
					 &= \calR(s,a) + \gamma \int_\S \calP(s,a,s') \sup_{a'} Q_*(s', a') ds'
		\end{split}
	\end{equation}
\end{proposition}
Again, these two equations are fixed-point equations and the existence and
uniqueness of a solution is guaranteed, under some technical assumptions, by
the contraction mapping theorem. Starting from the optimal value functions, we
can easily derived an optimal policy. Let us define a partial ordering in the
policy space
\begin{equation}
	\pi \succeq \pi' \Leftrightarrow V_\pi(s) \geq V_{\pi'}(s) \;\;\; \forall s \in \S
\end{equation}
Then the optimal policy $\pi_* \succeq \pi$, $\forall \pi$. We have the
following results
\begin{theorem}[Optimal Policy]
	For any Markov decision process,
	\begin{enumerate}[label={\roman*)}]
		\item It exists an optimal policy $\pi_*$ such that $\pi_* \succeq
			\pi$, $\forall \pi$.
		\item $V_{\pi_*}(s) = V_*(s)$.
		\item $Q_{\pi_*}(s,a) = Q_*(s,a)$.
	\end{enumerate}
\end{theorem}
An optimal policy can be found by acting greedily with respect to $Q_*$, which corresponds to selecting the action that maximizes the action-value function in a given state
\begin{equation}
	a_* = \argsup_{a\in\A} Q_*(s,a)
\end{equation}
This policy is deterministic and only depends on the current state of the system.

\subsection{Average Reward Formulation}
Most of the research in RL has studied a problem formulation where agents
maximize the cumulative sum of rewards. However, this approach cannot handle
infinite horizon tasks, where there are no absorbing goal states, without
discounting future rewards. Clearly, discounting is only necessary in cyclical
tasks, where the cumulative reward sum can be unbounded. More natural long-term
measure of  optimality exists for such cyclical tasks, based on maximizing the
average reward per action. For a more in-depth presentation, the reader may again refer to the extensive literature on the subject, such as \cite{arapostathis1993discrete}, \cite{mahadevan1996average} and the references therein. In the average reward setting, also known as long-run reward or ergodic reward, the goal of the agent is to find a policy that maximizes the expected reward per step.
\begin{definition}[Average Reward]
	The average reward $\rho_\pi$ associated to a policy $\pi$ is defined as
	\begin{equation}
		\begin{split}
			\rho_\pi &= \lim_{T\to\infty} \frac{1}{T} \E[\pi]{ \sum^{T-1}_{t=0} R_{t+1}}\\
					 &= \E[\substack{S\sim d_\pi \\ A \sim \pi}]{\calR(S,A)}\\
					 &= \int_\S d_\pi(s) \int_\A \pi(s, a) \calR(s,a) da ds\\
		\end{split}
	\end{equation}
where $d_\pi$ is the stationary distribution of the Markov process induced by $\pi$.
\end{definition}
The agent aims to find an \emph{average optimal} policy
\begin{equation}
	\pi_* = \argsup_\pi \rho_\pi
\end{equation}
In this setting, we introduce the \emph{average adjusted} value and action-value
functions.
\begin{definition}[Average Adjusted State-Value Function]
	The average adjusted state-value function $V_\pi : \S \to \R$ is the
	expected residual return that can be obtained starting from a state and
	following policy $\pi$
	\begin{equation}
		V_\pi(s) = \E[\pi]{\sum^{\infty}_{t=0} \left(R_{t+1} - \rho_\pi\right)
		\bigg| S_0 = s}
	\end{equation}
\end{definition}
The term $V_\pi(s)$ is usually referred to as the \emph{bias} value, or the
\emph{relative} value, since it represents the relative difference in total
reward gained starting from a state $s$ as opposed to a generic state.
$\rho_\pi$ serves as a baseline that allows to avoid divergence in the value
function definition.
\begin{definition}[Average Adjusted Action-Value Function]
	The average adjusted action-value function $Q_\pi : \S \times \A \to \R$ is
	the expected residual return that can be obtained starting from a state,
	taking an action and then following policy $\pi$
	\begin{equation}
		Q_\pi(s, a) = \E[\pi]{\sum^{\infty}_{t=0} \left(R_{t+1} -
			\rho_\pi\right) \bigg| S_0 = s, A_0
		= a}
	\end{equation}
\end{definition}
We have the following relation between the state-value function and the
action-value function
\begin{equation}\label{eq:VQ_equality}
	V_\pi(s) = \int_\A \pi(s,a) Q_\pi(s,a) da
\end{equation}
The value functions satisfy the following Bellman equation
\begin{proposition}[Bellman Expectation Equations]
	\begin{equation}
		V_\pi(s) = \calR_\pi(s) - \rho_\pi + T_\pi V_\pi(s)
	\end{equation}
	\begin{equation}
			Q_\pi(s,a) = \calR(s,a) - \rho_\pi + T_a V_\pi(s)
	\end{equation}
\end{proposition}
Again, by introducing opportune Bellman operators, these equations can be rewritten as fixed-point equations. In the discrete case, where the transition operator correspond to matrices, these Bellman equations become linear systems that can be solved to obtain the value functions.

\section{Risk-Sensitive Framework}
\label{sec:risk_sensitive_formulation}
In many application, in addition to maximizing the average reward, the agent
may want to control risk by minimizing some measure of variability in rewards. In the risk-sensitive framework, the goal of the agent is to find the policy that optimally solves the trade-off between reward and risk. Although risk-sensitive sequential decision-making has a long history in operations research and finance, it has only recently grabbed the attention of the machine learning community. Hence, the literature offers many reference which approach the risk-sensitive control problem from the traditional stochastic optimal control perspective. On the other hand, there are only few references that attack the problem in the reinforcement learning setting. Again, we can consider the discounted formulation or the average formulation.

\subsection{Discounted Reward Formulation}
A standard way to measure the risk associated with a policy $\pi$ is the variance of the total discounted reward obtained starting from a given state $s$
\begin{equation}
	\Lambda_\pi(s) = \Var[\pi]{G_t | S_t = s}
\end{equation}
This approach is the one considered in \cite{sobel1982variance}. The variance can be decomposed as
\begin{equation}
	\label{eq:variance_decomposition}
	\Lambda_\pi(s) = U_\pi(s) - V_\pi(s)^2
\end{equation}
where we denoted by $U_\pi$ the square state-value function.
\begin{definition}[Square State-Value Function]
	The square state-value function $U_\pi(s)$ is the second moment of the return that can be obtained under policy $\pi$ starting from a state $s$
	\begin{equation}
		U_\pi(s) = \E[\pi]{G_t^2 | S_t = s}
	\end{equation}
\end{definition}
As for the risk-neutral formulation, it comes in handy to introduce a square action-value function.
\begin{definition}[Square Action-Value Function]
	The square action-value function $W_\pi(s)$ is the second moment of the return that can be obtained starting from a state $s$, taking action $a$ and then following policy $\pi$
	\begin{equation}
		W_\pi(s, a) = \E[\pi]{G_t^2 | S_t = s, A_t = a}
	\end{equation}
\end{definition}
Clearly, the two square value functions are related by the following equation
\begin{equation}
	U_\pi(s) = \int_{\A} \pi(s,a) W_\pi(s,a) da
\end{equation}
We would like to obtain a Bellman expectation equation for $U_\pi(s)$ and $W_\pi(s,a)$, in order to piggyback on the discussion about the standard value functions. To do so, let us introduce the square reward function.
\begin{definition}[Square Reward Function]
	The square reward function $\calM(s,a)$ is the second moment of the reward that can be obtained in state $s$ when taking action $a$
	\begin{equation}
		\calM(s,a) = \E{R_{t+1}^2 | S_t = s, A_t = a}
	\end{equation}
	Let us also define the state-reward function $\calM_\pi(s)$
	\begin{equation}
		\calM_\pi(s) = \E[\pi]{R_{t+1}^2|S_t = s} = \int_\A \pi(s,a) \calM(s,a) da
	\end{equation}
\end{definition}
Moreover, we will need the reward-return state-covariance function.
\begin{definition}[Reward-Return State-Covariance Function]
	The reward-return state-covariance function $C_\pi(s)$ is the covariance between the first day reward and the successive returns starting from state $s$ and then following policy $\pi$
	\begin{equation}
		C_\pi(s) = \Cov[\pi]{R_{t+1}, G_{t+1} | S_t = s}
	\end{equation}
\end{definition}
Again, it comes in handy to also introduce the reward-return action-covariance function.
\begin{definition}[Reward-Return Action-Covariance Function]
	The reward-return action-covariance function $C_\pi(s,a)$ is the covariance between the first reward and the successive return starting from state $s$, taking action $a$ and then following policy $\pi$
	\begin{equation}
		C_\pi(s,a) = \Cov[\pi]{R_{t+1}, G_{t+1} | S_t = s, A_t = a}
	\end{equation}
\end{definition}
Then, it is easy to show that the square state-value function satisfies the following Bellman equation
\begin{proposition}[Bellman Expectation Equation]
\begin{equation}
	U_\pi(s) = \calK_\pi(s) + \gamma^2 T_\pi U_\pi(s)
\end{equation}
\begin{equation}
	\begin{split}
		W_\pi(s,a) = \calK_\pi(s, a) + \gamma^2 T_a U_\pi(s)
	\end{split}
\end{equation}
where we denoted
\begin{equation}
	\mathcal{K}_\pi(s) = \calM_\pi(s) + 2\gamma \calR_\pi(s) T_\pi V_\pi(s) + 2 \gamma C_\pi(s)
\end{equation}
\begin{equation}
	\calK_\pi(s, a) = \calM(s,a) + 2\gamma \calR(s,a) T_a V_\pi(s) + 2 \gamma C_\pi(s, a)
\end{equation}
\end{proposition}
\begin{proof}
	Let us prove the Bellman expectation equation for the square action-value function. The proof for the square state-value function is analogous.
	\begin{equation*}
		\begin{split}
			W_\pi(s,a) &= \E[\pi]{G_t^2 | S_t = s, A_t = a}\\
					   &= \E[\pi]{(R_{t+1} + \gamma G_{t+1})^2 | S_t = s, A_t = a}\\
					   &= \calM(s,a) + 2\gamma \E[\pi]{R_{t+1}G_{t+1} | S_t = s, A_t = a} + \gamma^2 T_a U_\pi(s)
		\end{split}
	\end{equation*}
	By the definition of covariance
	\begin{equation*}
			C_\pi(s,a) = \E[\pi]{R_{t+1}G_{t+1} | S_t = s, A_t = a} - \calR(s,a) T_a V_\pi(s)
	\end{equation*}
	Plugging in the first equation leads to the result.
\end{proof}
These equations are analogous to the Bellman expectation equations for the state-value function, with a synthetic reward function $\mathcal{K}_\pi(s)$ and a discount factor $\gamma^2$. It is possible to combine the Bellman expectation equation for $V_\pi(s)$ and for $U_\pi(s)$ to obtain a Bellman equation for the return variance $\Lambda_\pi(s)$.
\begin{proposition}[Bellman Expectation Equation]
	\begin{equation}
			\Lambda_\pi(s) = \mathcal{V}_\pi(s) + 2 \gamma C_\pi(s) + \gamma^2 \Var[\pi]{V_\pi(S_{t+1}) | S_t = s} + \gamma^2 T_\pi \Lambda_\pi(s)
	\end{equation}
	where $\mathcal{V}_\pi(s)$ denotes the conditional variance of the reward
	\begin{equation}
		\mathcal{V}_\pi(s) = \Var[\pi]{R_{t+1} | S_t = s} = \calM_\pi(s) - \calR_\pi(s)^2
	\end{equation}
\end{proposition}
\begin{proof}
The result can be proved starting from Eq. (\ref{eq:variance_decomposition}) and exploiting the Bellman equations for $U_\pi$ and $V_\pi$. Here we follow an alternative way based on the law of total variance.
\begin{equation*}
\begin{split}
		\Lambda_\pi(s) &= \Var[\pi]{G_t | S_t = s}\\
					   &= \Var[\pi]{R_{t+1} + \gamma G_{t+1} | S_t = s}\\
					   &= \mathcal{V}_\pi(s) + 2 \gamma \Cov[\pi]{R_{t+1}, G_{t+1} | S_t = s} + \gamma^2 \Var[\pi]{G_{t+1} | S_t = s}\\
\end{split}
\end{equation*}
Applying the law of total variance we obtain
\begin{equation*}
	\begin{split}
	\Var[\pi]{G_{t+1} | S_t = s} &= \E[\pi]{ \Var[\pi]{G_{t+1} | S_{t+1}} | S_t = s} + \Var[\pi]{\E[\pi]{G_{t+1} | S_{t+1}} | S_t = s}\\
	&= T_\pi \Lambda_\pi(s) + \Var[\pi]{V_\pi(S_{t+1}) | S_t = s}
	\end{split}
\end{equation*}
Plugging it into the first equality yields the result
\begin{equation*}
	\Lambda_\pi(s) = \Var[\pi]{R_{t+1} | S_t = s} + \Var[\pi]{V_\pi(S_{t+1}) | S_t = s} + \gamma^2 T_\pi \Lambda_\pi(s)
\end{equation*}
\end{proof}
Even if appealing from a theoretical point of view, these equations cannot be easily exploited to derive practical algorithms in a non-episodic environment. Indeed, the covariance term between the first day reward and the future return appearing in the synthetic reward would be hard to estimate when the lifespan of the experiment can be infinite. This does not necessarily represent a problem in an episodic environment, where the experiments have a finite (possibly random) lifespan. Even more problematic is the variance of the state-value function, which typically is unknown. For this reason, we will not develop any reinforcement learning algorithm in this framework and we will instead focus on the average reward formulation.\\
In \cite{tamar2012policy} and \cite{prashanth2014actor}, the authors circumvent these difficulties by implicitly assuming that the reward $R_{t+1}$ is conditionally independent from the future rewards given the current state. Under this assumption, the covariance terms are null and the previous Bellman equation become
\begin{corollary}[Bellman Equations Under Independence Assumption]
	\begin{equation}
		U_\pi(s) = \calM_\pi(s) + 2\gamma \calR_\pi(s) T_\pi V_\pi(s) + \gamma^2 T_\pi U_\pi(s)
	\end{equation}
	\begin{equation}
		W_\pi(s,a) = \calM(s,a) + 2\gamma \calR(s,a) T_a V_\pi(s) + \gamma^2 T_a U_\pi(s)
	\end{equation}
	\begin{equation}
		\Lambda_\pi(s) = \mathcal{V}_\pi(s) + \gamma^2 \Var[\pi]{V_\pi(S_{t+1}) | S_t = s} + \gamma^2 T_\pi \Lambda_\pi(s)
	\end{equation}
\end{corollary}

\subsection{Average Reward Formulation}
In \cite{prashanth2014actor}, the authors consider the long-run variance as a measure of the risk associated to a policy $\pi$
\begin{definition}[Long-Run Variance]
	The long-run variance $\Lambda_\pi$ under policy $\pi$ is defined as
	\begin{equation}
		\begin{split}
			\Lambda_\pi &= \lim_{T \to \infty} \frac{1}{T} \E[\pi]{
			\sum^{T-1}_{t=0} (R_{t+1} - \rho_\pi)^2}\\
		\end{split}
	\end{equation}
\end{definition}
The long-run variance can be decomposed as follows
\begin{equation}
	\Lambda_\pi = \eta_\pi - \rho_\pi^2
\end{equation}
where $\eta_\pi$ is the average square reward per step
\begin{definition}[Average Square Reward]
	\begin{equation}
		\begin{split}
			\eta_\pi &= \lim_{T\to\infty}\frac{1}{T}\E[\pi]{\sum_{t=0}^{T-1} R_{t+1}^2}\\
					 &= \E[\substack{S\sim d_\pi \\ A \sim \pi}]{\calM(S,A)}\\
					 &= \int_\S d_\pi(s) \int_\A \pi(s,a) \calM(s,a) da ds\\
		\end{split}
	\end{equation}
	where we denoted by $\calM(s,a)$ the square reward function
	\begin{equation}
		\calM(s,a) = \E{R_{t+1}^2 | S_t = s, A_t = a}
	\end{equation}
\end{definition}
As before, we introduce the residual state-value and action-value functions
associated with the square reward under policy $\pi$
\begin{definition}[Average Adjusted Square State-Value Function]
	The average adjusted square state-value function $U_\pi : \S \to \R$ is the
	expected square residual return that can be obtained starting from a state
	and following policy $\pi$
	\begin{equation}
		U_\pi(s) = \E[\pi]{\sum^{\infty}_{t=0} \left(R_{t+1}^2 - \eta_\pi\right)
		\bigg| S_0 = s}
	\end{equation}
\end{definition}
\begin{definition}[Average Adjusted Square Action-Value Function]
	The average adjusted square action-value function $Q_\pi : \S \times \A \to
	\R$ is the expected residual square return that can be obtained starting from a
	state, taking an action and then following policy $\pi$
	\begin{equation}
		W_\pi(s, a) = \E[\pi]{\sum^{\infty}_{t=0} \left(R_{t+1}^2 -
			\eta_\pi\right) \bigg| S_0 = s, A_0 = a}
	\end{equation}
\end{definition}
The following relation between square state-value function and the square action-value holds
\begin{equation}\label{eq:UW_equality}
	U_\pi(s) = \int_\A \pi(s,a) W_\pi(s,a) da
\end{equation}
The average adjusted square value functions satisfy the following Bellman equations
\begin{proposition}[Bellman Expectation Equations]
	\begin{equation}
		U_\pi(s) = \calM_\pi(s) - \eta_\pi + T_\pi U_\pi(s)
	\end{equation}
	\begin{equation}
		\begin{split}
			W_\pi(s,a) &= \calM(s,a) - \eta_\pi + T_a U_\pi(s)\\
			&= \calM(s,a) - \eta_\pi + \int_{\S} \calP(s,a,s') \int_{\A} \pi(s',a') W_\pi(s',a') da' ds'
		\end{split}
	\end{equation}
\end{proposition}
Now, how can we formally define the agent's objective to take into account the trade-off between rewards and risks? Borrowing from the financial literature, a first alternative is to consider the following mean-variance optimization problem
\begin{equation}\label{eq:risk_sensitive_problem}
	\begin{cases}
		\max_\pi \rho_\pi\\
		\text{subject to}\ \Lambda_\pi \leq \alpha\\
	\end{cases}
\end{equation}
for a given $\alpha > 0$. Using the Lagrangian relaxation procedure, we can
recast (\ref{eq:risk_sensitive_problem}) to the following unconstrained problem
\begin{equation}
	\max_\lambda \min_\pi L(\pi, \lambda) = - \rho_\pi + \lambda
	(\Lambda_\theta - \alpha)
\end{equation}
Alternatively, the agent may want to optimize the Sharpe ratio, a commonly used risk-sensitive performance measure defined as
\begin{equation}
	\text{Sh}_\pi = \frac{\rho_\pi}{\sqrt{\Lambda_\pi}}
\end{equation}

\section{Dynamic Programming Algorithms}
\label{sec:policy_evaluation}
In discrete-time, the Bellman optimality equations can be seen as an expression of the dynamic programming principle and completely characterize the optimal value functions. In the case of discrete state and action spaces, the value functions can be represented as vectors and the Bellman operators become matrices so that the optimality equations can be directly solved in polynomial time \cite{littman1996algorithms}. An alternative approach is to approximate the optimal value functions by using the Bellman optimality equations as the basis of a fixed-point iterative scheme. This leads to the well known \emph{value iteration} and \emph{policy iteration} algorithms \cite{szepesvari2010algorithms}. The drawback of these methods is that typically they are only applicable to discrete MDP whose dynamics is perfectly known. Still, these simple algorithms provide some useful insight that can be exploited to develop more advanced reinforcement learning algorithm.

\subsection{Value Iteration}
\emph{Value iteration} is an iterative method to compute the optimal state-value function $V_*(s)$. Starting from an arbitrary function $V_0(s)$, the algorithm iteratively updates the approximation by applying the Bellman optimality operator in a fixed-point scheme
\begin{equation*}
	V_{k+1}(s) = B_* V_k(s) = \sup_a \left\{\calR(s,a) + \gamma T_a V_k(s)\right\}
\end{equation*}
This algorithm is guaranteed to converge to the optimal value function $V_*$ by the contraction mapping theorem. Let us notice that the transition operator $T_a$ requires knowledge of the MDP dynamics, which is not available in the typical reinforcement learning framework. Moreover, the update involves an optimization with respect to all possible actions which can be carried out efficiently only in the case of finite action spaces. A drawback of this algorithm is that it does not provide an explicit representation of the optimal policy, which in many control problems is what we are looking for.

\subsection{Policy Iteration}
\begin{figure}[t]
	\centering
	\includegraphics[width=0.3\textwidth]{Images/2_0_policy_iteration.png}
	\caption[Policy iteration algorithm.]{Policy iteration algorithm \cite{sutton1998introduction}.}
	\label{fig:policy_iteration}
\end{figure}
Policy iteration is an iterative method to approximate both the optimal state-value function $V_*$ and the optimal policy $\pi_*$. This algorithm alternates an evaluation step, in which the current policy is evaluated using the state-value function, and an improvement step, in which the policy is improved by acting greedily with respect to the action-value function computed in the evaluation step. In the standard version of policy iteration, the state-value function for the current policy is evaluated starting from an arbitrary function $V_0(s)$ and iteratively applying the Bellman expectation operator in a fixed-point iteration scheme
\begin{equation*}
	V_{k+1}(s) = B_{\pi_k} V_k(s)
\end{equation*}
This algorithm is guaranteed to converge to $V_{\pi_*}$. The new policy is then computed as
\begin{equation*}
	\pi_{k+1} = \text{greedy}(V_{\pi_k})
\end{equation*}
and we go back to the evaluation step for this new policy. Let us notice that it is not necessary to perfectly evaluate to policy $\pi_n$ before performing the improvement step and the evaluation procedure can be stopped before convergence. This algorithm suffers from the same problem as above. However it provides the basic structure for most of the value-based reinforcement learning methods. In particular, in \emph{generalized policy iteration} the evaluation step is performed in an arbitrary way which does not necessarily employ the Bellman operator. This scheme is illustrated in Figure \ref{fig:policy_iteration},


%%%%%PLEASE CONSIDER CORRECTIONS AT PLACES INDICATED%%%%%%%%
\documentclass{discussion}
%%%%%Packages Used, add more if necessary%%%%
\providecommand{\tightlist}{%
\setlength{\itemsep}{2pt}\setlength{\parskip}{0pt}}

\begin{document}

%%%%%CHANGE HERE%%%%%%%%%
%%%%%\lecture{the ordinal number of the tutorial}{lecture title}{Instructor's name}%%%%%%%%
\lecture{5}{Naive Bayes and SVM}{Chansoo Lee}{}

%%%%%CHANGE HERE%%%%%%%
%%%%%\section{title of the section} similarly with the rest \section{} or \subsection{} or \subsubsection{} etc

\renewcommand{\R}{\mathbb{R}}
\renewcommand{\vec}[1]{\boldsymbol{#1}}
\newcommand{\Dtrain}{\mathcal{D}_{\mathrm{train}}}
\newcommand{\tm}{\mathbf{\theta}_{\text{model}}}
\newcommand{\xtest}{\mathbf{\x}_{\text{test}}}
\newcommand{\test}{\mathrm{test}}
\newcommand{\pred}{\mathrm{pred}}

\section{Naive Bayes}
\emph{Notations:} Let $C$ be number of classes, $D$ be the number of features, and $M$ be the number of values each feature can take. The Naive Bayes prior parameters are $\vec{\pi}$ and posterior parameters are $\{\vec{\theta}_{cd}: c=1,\ldots,C, d = 1,\ldots,D\}$. For simplicity we simply use $\pi, \theta$ when discussing them as general parameters and thus dimensions are not important in the context.
\subsection{Naive Bayes: MLE}

\begin{exercise}[Review Question]
 What are the semantics of these parameters? What are the dimensions of $\vec{\pi}$ and $\vec{\theta}_{cd}$? They are called \emph{probability vectors}. What is the key property?

 \textit{The individual elements of a probability vector sums up to 1.}
\end{exercise}

Furthermore, suppose $M=2$. When dealing with sparse features such that $x_{d}$ is rarely 1 (\texttt{Spambase} data in HW2, after our preprocessing step, has this property), we only see a small number of examples to accurately estimate $\theta_{cd1}$.  This causes overfitting. (\emph{Understand exactly why this is the case.})

\begin{exercise}
\label{ex:always1}
  Discuss what happens if $x_d = 1$ for every training example $x$, regardless of its class label? What happens if we recieve a test data with $x_d = 0$?
\end{exercise}

\subsection{Naive Bayes: Mean estimate}
The overfitting problem can be fixed by putting Dirichlet priors on $\vec{\pi}$. Dirichlet distribution over $M$-dimensional probability vector, parameterized by a vector $\vec{\alpha} \in \mathbb{R}^{M}$ of positive values, has the following probability density function:
\[p(\vec{u}; \vec{\alpha}) = Z(\vec{\alpha}) u_{1}^{\alpha_1 - 1} \cdots u_{M}^{\alpha_{M}-1}\]
where $Z(\vec{\alpha})$ is the normalizing constant, which makes the above function integrates to 1.

\begin{exercise} Let's inspect the Dirichlet pdf.
\begin{itemize}
  \item   Fix every coordinates but $\alpha_1$, and increase $\alpha_1$. What happens to the distribution of $u_1$? What would happen to the mean of $u_1?$.

  \textit{We are more likely to get a high value, so the mean would increase.}
  \item Set all coordinates of $\vec{\alpha}$ to $\lambda$. As you increase $\lambda$, what would happen to the expected value of $\vec{u}$?

  \textit{The expected value is always $\vec{u} = (1/M, \ldots, 1/M)^\top$  because the sum $\sum_{i=1}^{M}u_i$ is constrained to be 1 and pdf is symmetric.}
\end{itemize}
\end{exercise}

With Dirichlet prior, we can do the MAP estimate as usual
\[(\hat{\pi}, \hat{\theta}) = \arg\max_{\pi,\theta} P(\theta,\pi | \mathcal{D}_{\text{train}}).\]
But unfortunately the expression for the maximizer isn't very pretty (functionally they are fine). So, we use the mean estimate instead:
\[\hat{\pi} = E[\pi| \mathcal{D}_{\text{train}}], \ \
 \hat{\theta} = E[\theta | \mathcal{D}_{\text{train}}]\]
where $\vec{\pi} \sim \text{Dirichlet}(\vec{\alpha})$ and $\theta_{cd} \sim \text{Dirichlet}(\vec{\beta}_{cd}).$ If we compute the means, we get
\[\hat{\pi}_c = \frac{N_c + \alpha_c}{N + \sum_{c} \alpha_c}, \ \
\hat{\theta}_{cdm} = \frac{N_{cdm} + \beta_{cdm}}{N_c + \sum_{m'} \beta_{cdm'}} \]

\begin{exercise} Redo Exercise \ref{ex:always1}. What happens at the prediction time?
\end{exercise}

\section{SVM}
\newcommand{\w}{\mathbf{w}}
\renewcommand{\x}{\mathbf{x}}
\renewcommand{\c}[1]{t_{#1} (\w^\top \x_{#1}) + b}
\newcommand{\ci}{\c{i}}
Let $\{(\x_i, t_i) \in \mathbb{R}^D \times \{-1,1\}\}_{i=1}^{N}$ be our training set.

Hyperplane is a \emph{set} of $D$-dimensional vectors that ``constitute'' an $(D-1)$ dimensional object. The key here is \emph{set}, as a hyperplane is usually described in the form of:
\[\{\vec{v}: \mathbf{w}^\top \vec{v} + b = 0\}.\]
The distance between the above hyperplane and a point $\mathbf{x}$ is
\[\frac{|\mathbf{w}^\top \mathbf{x} + b|}{\|\mathbf{w}\|}.\]

% Throught the section, we use the shorthand notation $\ci = t_i (\w^\top \x_i) + b$. Don't forget it is a function of both the model $(\w^\top,b)$ and data $(\x_i, t_i)$.

\subsection{Hard SVM}
The Hard SVM maximizes the \emph{margin distance}, which is the minimum distance between a point and the hyperplane $\{\vec{v}: \mathbf{w}^\top \vec{v} + b = 0\}$:
\begin{equation}
\label{eq:hardsvm}
  \arg\max_{\mathbf{w}, b} \min_{i = 1,\ldots, N} \frac{|\mathbf{w}^\top \mathbf{x}_i + b|}{\|\mathbf{w}\|} \ \ \text{such that $\ci > 0$ for all $i$.}
\end{equation}
\begin{exercise}(Review)Does the solution always exist? What is the condition under which the solution exists?
\end{exercise}

When the solution exists, \eqref{eq:hardsvm} is equivalent to
\begin{equation}
\label{eq:hardsvmci}
  \arg\max_{\mathbf{w}, b} \min_{i = 1,\ldots, N} \frac{\ci}{\|\mathbf{w}\|} \ \ \text{such that $\ci > 0$ for all $i$.}
\end{equation}

\begin{exercise}
Show the equivalence of \eqref{eq:hardsvm} and \eqref{eq:hardsvmci}.
\end{exercise}

\begin{exercise}(Review) Given a solution $(\w^*,b^*)$ to the above, can we generate another solution?
\emph{Hint: As you scale $(\w^*,b^*)$ with $\gamma$ and set $\w = \gamma\w^*$ and $b = \gamma b^*$, what happens to $t_i (\w^\top \x_i) + b$?}
% \emph{You can scale both with a constant $\gamma$ and objective function value remains the same.}
\end{exercise}

By fixing $\c{i^*} = 1$ for the minizer $i^*$, we get an optimization problem whose unique solution is the maximizer of (to be precise, \emph{one of} the maximizers of) \eqref{eq:hardsvmci}:
\begin{equation}
    \arg\max_{\mathbf{w}, b} \min_{i = 1,\ldots, N} \frac{\ci}{\|\mathbf{w}\|}  \ \text{such that $\ci = 1$ for some $i$ and $\ci \geq 1$ for all other $i$.}
\end{equation}

The above is equivalent to
\begin{equation}
    \arg\max_{\mathbf{w}, b} \frac{1}{\|\mathbf{w}\|}  \ \text{such that $\ci = 1$ for some $i$ and $\ci \geq 1$ for all other $i$.}
\end{equation}
which is again equivalent to
\begin{equation}
    \arg\min_{\mathbf{w}, b} \|\mathbf{w}\|  \ \text{such that $\ci = 1$ for some $i$ and $\ci \geq 1$ for all other $i$.}
\end{equation}

\begin{exercise}
Show that the above is equivalent to
\begin{equation}
\label{eq:hardsvm_primal}
    \arg\min_{\mathbf{w}, b} \|\mathbf{w}\|  \ \text{such that $\ci \geq 1$ for all $i$.}
\end{equation}

\emph{Proof technique: Suppose our minimizer $(\w^*, b^*)$ gives $\min_i t_i({\w^*}^\top \x_i) > 1$. Can you find a better $(\w, b)$ and get a contradictiction?}
\end{exercise}

Now we can change $\|\w\|$ into any increasing function of $\|\w\|$ (like we changed likelihood to log likelihood). So why not something that looks exactly like our regularizer for linear and logistic regression?
\begin{equation}
    \arg\min_{\mathbf{w}, b} \lambda\|\mathbf{w}\|^2  \ \text{such that $\ci \geq 1$ for all $i$.}
\end{equation}
where $\lambda >0 $ is the regularization coefficient.

\subsection{Soft-SVM}

We relax the problem so that we allow $c_i$ to be less than 1 (\textbf{margin error}), but we add a penalty term.

\begin{equation}
    \arg\min_{\mathbf{w}, b, \vec{\xi}} \lambda\|\mathbf{w}\|^2 + \sum_{i=1}^{N} \xi_i \ \text{such that $\ci \geq 1 - \xi_i$ and $\xi_ \geq 0$ for all $i$.}
\end{equation}

In the HW, you will show that the above is equivalent to

\newcommand{\Lhinge}{L_{\text{hinge}}}
\begin{equation}
\label{eq:svm_primal}
    \arg\min_{\mathbf{w}, b} \lambda\|\mathbf{w}\|^2 + \sum_i \max(0,1 - (\ci)).
\end{equation}
% where $\Lhinge(\w;\x_i) =\max(0,1 - \ci)$
\begin{exercise}
  Plot the \textbf{hinge loss} function $f(z) = \max(0, 1 - z)$. What does it look like? What does the SVM penalty function do intuitively? How is it different from RMSE?
\end{exercise}

\subsection{Duality and Kernel}

The dual formulation optimizes over a vector in $\mathbb{R}^N$, instead of $\mathbb{R}^D$.
    \begin{align*}
    \underset{\vec{\alpha}}{\text{maximize}} \quad &  -0.5 \sum \nolimits_{i,j = 1}^N \alpha_i \alpha_j t_i t_j \x_i^\top \x_j + \sum \nolimits_{i = 1}^N \alpha_i\\
    \text{subject to} \quad & 0 \leq \alpha_i \leq C/n \quad \forall i\ \\
    \quad & \sum \nolimits_{i=1}^n \alpha_i t_i = 0
    \end{align*}
Given a solution to the dual problem, we can get the primal solution, and vice versa. The main (arguably the only) reason we study the SVM dual formulation is that it helps us observe that the optimal  $\w^*$ is a linear combination of examples:
\[\boxed{ \w^* = \sum \nolimits_{i=1}^N \alpha_i^* t_i \x_i }\]
The name ``support vector'' comes from this observation.

Because we know that the optimal $\w^*$ is a linear combination of $\x_i$, we can rewrite \eqref{eq:svm_primal} as:
\begin{equation}
    \arg\min_{\vec{\alpha},b} \lambda\|\mathbf{w}\|^2 + \sum_{i=1}^{N} \max\left(0, 1 - t_i\left( \left(\sum_{j=1}^{N} \alpha_j \x_j\right)^\top \x_i + b\right)\right).
\end{equation}

Using kernelized features, we get
\begin{equation}
    \arg\min_{\vec{\alpha}} \lambda\|\mathbf{w}\|^2 + \sum_{i=1}^{N} \max\left(0, 1 - t_i \left(\sum_{j=1}^{N} \alpha_j \phi(\x_j)\right)^\top \phi(\x_i)\right).
\end{equation}
The bias term is removed because we can always add a bias coordinate in our feature mapping $\phi(\x_j)$.

\begin{exercise}
  Take the gradient of the objective function. In particular, take the derivative with respect to $\alpha_k$.
\end{exercise}

Observe that the derivative depends only on the dot product of the features. So we can implement the gradient descent as long as the dot products are well-defined, even if we are in infinite-dimensional space.

\end{document}