\documentclass[11pt]{article}
\usepackage{latexsym}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsthm}
\usepackage{epsfig}
\usepackage[tight]{subfigure}

\newcommand{\handout}[5]{
  \noindent
  \begin{center}
  \framebox{
    \vbox{
      \hbox to 5.78in { {#1} \hfill #2 }
      \vspace{4mm}
      \hbox to 5.78in { {\Large \hfill #5  \hfill} }
      \vspace{2mm}
      \hbox to 5.78in { {\em #3 \hfill #4} }
    }
  }
  \end{center}
  \vspace*{4mm}
}

\newcommand{\lecture}[5]{\handout{#1}{#2}{#3}{#4}{#5}}

\newtheorem{theorem}{Theorem}
\newtheorem{corollary}[theorem]{Corollary}
\newtheorem{lemma}[theorem]{Lemma}
\newtheorem{observation}[theorem]{Observation}
\newtheorem{proposition}[theorem]{Proposition}
\newtheorem{definition}[theorem]{Definition}
\newtheorem{claim}[theorem]{Claim}
\newtheorem{fact}[theorem]{Fact}
\newtheorem{assumption}[theorem]{Assumption}

% 1-inch margins, from fullpage.sty by H.Partl, Version 2, Dec. 15, 1988.
\topmargin 0pt \advance \topmargin by -\headheight \advance \topmargin by -\headsep \textheight 8.9in
\oddsidemargin 0pt \evensidemargin \oddsidemargin \marginparwidth 0.5in \textwidth 6.5in

\parindent 0in
\parskip 1.5ex
%\renewcommand{\baselinestretch}{1.25}

\begin{document}

\lecture{Statistical Techniques in Robotics (16-831, F09)}{Lecture \#18 (Thursday October 22)}{Lecturer: Drew
Bagnell}{Scribe: Siddharth Mehrotra}{Gauss Markov Filter}


\section* {Gauss Markov Filter}
Consider $X_1,X_2,....X_t,X_{t+1}$ to be the state variables and $Y_1,Y_2,...Y_t,Y_{t+1}$ be the sequence of corresponding observations. As in Hidden Markov models, conditional independencies (see Figure 1) dictate that past and future states are decorrelated given the current state, $X_t$ at time $t$. For example, if we know what $X_2$ is, then no information
about $X_1$ can possibly help us to reason about what $X_3$ should be. 
\begin{figure}[h]
\centering
   \includegraphics[width=12cm, angle=0, trim=0.25in 2.5in 0.25in 2.5in]{1.pdf}
\caption{The Independence Diagram of a Gauss-Markov model}
\end{figure}


The update for state variable $X_{t+1}$ is given by:
\[X_{t+1} = AX_t + \epsilon\]
where,
\[\epsilon \sim N(0,Q)\]
\[\Rightarrow X_{t+1}|X_t \sim N(AX_t, Q)\]
The corresponding observation $Y_{t+1}$ is given by equation:
\[Y_{t+1} = CX_{t+1} + \delta\]
where,
\[\delta \sim N(0,R)\]
\[\Rightarrow Y_0 \sim N(\mu_0,\epsilon_0)\]

\section* {Lazy Gauss Markov Filter}
\subsection* {Motion Model:} 
Before the observation is taken:
\[X_{t+1} \sim \mu_{t+1}^- = A\mu_t\]
\subsection* {Proof:}
\[E[X_{t+1}] = E[AX_t + \epsilon]\]
\[\Rightarrow E[X_{t+1}] = E[AX_t] + E[\epsilon]\]
since variance of $\epsilon$ is 0,
\[\Rightarrow E[X_{t+1}] = AE[X_t] = A\mu_t\] 
Variance,
\[\Sigma_{t+1}^- = E[X_{t+1}*X_{t+1}{}^T]\]
\[\Rightarrow \Sigma_{t+1}^- = E[(AX_t + \epsilon)(AX_t + \epsilon)^T]\] 
\[ = E[(AX_t)(AX_t)^T] + E[\epsilon_{terms}]\] 
\[ = AE[(X_t)(X_t)^T]A^T + E[\epsilon_{terms}]\]
\[\Rightarrow \Sigma_{t+1}^- = A\Sigma_tA^T + E[\epsilon_{terms}]\]
E[$\epsilon_{terms}$] is equal to the variance of $\epsilon$ which is $Q$.\\ 
Therefore Motion Update becomes:

\[\mu_{t+1}^- = A\mu_t\]
\[\Sigma_{t+1}^- = A\Sigma_tA^T + Q\]

\subsection*{Observation Model:}
For the observation model Natural parameterization is more suitable as it involves multiplication of terms.
When, $Y$ is the corresponding observation for state variable $X$, the model equation in terms of Natural Parameters $J$ and $P$ is given by, 
\[e^{(J^{-T}X - \frac{1}{2}X^TPX)}*e^{-\frac{1}{2}(Y - CX)^TR^{-1}(Y-CX)}\]
\[\Rightarrow e^{-\frac{1}{2}[-2Y^TR^{-1}CX + X^TC^TR^{-1}CX + Y^TR^{-1}Y]}\]
  
The last term is a constant with respect to X, so it goes into the marginalization term.

\[\Rightarrow e^{-\frac{1}{2}[-2Y^TR^{-1}CX + X^TC^TR^{-1}CX]}\]

Therefore the Observation Update is:

\[ J^+ = J^- + (Y^TR^{-1}C)^T\]
\[ P^+ = P^- + C^{-1}R^{-1}C\]
This form is useful when there are large number of motion and observation updates.\\
Lazy Gauss Markov can be expressed in two forms:
\begin{itemize}
    \item When expressed in terms of moment parameters $\mu$ and $\Sigma$ acts as Kalman Filter.
		\item When expressed in terms of natural parameters $J$ and $P$ acts as Information Filter.
\end{itemize}

\subsection*{Observation Update in terms of moment parameters $\mu$ and $\Sigma$:}

\[ \left( \begin{array}{c}
X_t\\
Y_t\\
\end{array} \right) \sim N \left( \begin{array}{cc}
\left( \begin{array}{c}
\mu_{X_t} \\
\mu_{Y_t}\\
\end{array} \right) &  \left( \begin{array}{cc}
\Sigma_{XX} & \Sigma_{YX} \\
\Sigma_{XY} & \Sigma_{YY}\\
\end{array} \right)   \\
\end{array} \right) \]


Observation Update:

\[ \mu_{X|Y} = \mu_X + \Sigma_{XY}\Sigma_{YY}^{-1}(Y - \mu_Y)\leftarrow(1)\]
\[ \Sigma_{X|Y} = \Sigma_{XX} - \Sigma_{XY}\Sigma_{YY}^{-1}\Sigma_{YX}\leftarrow(2)\]
$(Y - \mu_Y)$ is called the Innovation Term.

We know,
\[ \mu_Y = C\mu_X\]
\[ \Sigma_{YY} = R + C\Sigma_{XX}C^T\]
Therefore, we have to find out $\Sigma_{XY}$ to calculate the remaining terms in equation $1$ and $2$. \\
By definition,
\[ \Sigma_{XY} = E[(X - \mu_X)(Y - \mu_Y)^T]\]
\[= E[(X - \mu_X)(Y - C\mu_X)^T]\]
However, $Y = CX + \delta$ with $\delta$ having 0 mean and independent of all other observations.
\[ \Sigma_{XY} = E[(X - \mu_X)(X - \mu_X)^T]C^T\]
\[\Rightarrow \Sigma_{XY} = \Sigma_{XX}C^T\]
Putting, these values in equations $1$ and $2$,
\[ \mu_{X|Y} = \mu_X + \Sigma_{XX}C^T(R+C\Sigma_{XX}C^T)^{-1}(Y - C\mu_X)\]
\[ \Sigma_{X|Y} = \Sigma_{XX} - \Sigma_{XX}C^T(R+C\Sigma_{XX}C^T)^{-1}C\Sigma_{XX} \]

\subsection* {KF to BLR transformation:}
The corresponding parameters for converting the above KF form to BLR is given by:
\[ BLR\left( \begin{array}{c}
\Theta = X\\
\sigma^2 = R\\
0 = Q\\
I = A\\
Y = Y\\
X = C_t\\
\end{array} \right)KF\]
 
 \end{document}
