\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, F10)}{Lecture \#19 (Tuesday October 26)}{Lecturer: Drew
Bagnell}{Scribe: Kiho Kwak, Siddharth Mehrotra}{Kalman Filtering (part 1)}


\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]{./Fig/GaussMarkovmodel.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)\]

Each component is defined as follow:
\begin{itemize}
	\item $A_{t}$: Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise.
	\item $C_t$: Matrix (kxn) that describes how to map the state $X_{t}$ to an observation $Y_{t}$.
	\item $\epsilon_{t}$, $\delta_{t}$: Matrix (nxn) Random variables representing the process and measurement noise that are assumed to be independent and 	normally distributed with covariance $R_{t}$ and $Q_{t}$ respectively.
\end{itemize}



\section* {Lazy Gauss Markov Filter}
\subsection* {Motion Model (Prediction step):} 
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 (Correction step):}
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 {\textbf{ Kalman Filter}}.
		\item When expressed in terms of natural parameters $J$ and $P$ acts as {\textbf{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)\]
$ \Sigma_{XY}\Sigma_{YY}^{-1}$ is Kalman gain, $K_{t}$ and $(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} \]


\begin{figure}[h]
\centering
  	 \includegraphics[width=10cm]{./Fig/kalmanfilterAlgorithm.pdf}
	\caption{The Kalman filter algorithm}
\end{figure}

\begin{figure}[t]
\centering
   	\includegraphics[width=13cm]{./Fig/kalmanfilter.pdf}
	\caption{Illustration of Kalman filters: (a) initial belief, (b) a measurement (in bold) with the associated uncertainty, (c) belief after integrating the measurement into the belief using the Kalman filter algorithm, (d) belief after motion to the right (which introduces uncertainty), (e) a new measurement with associated uncertainty, and (f) the resulting belief.}
\end{figure}

 
 \end{document}
