概率机器人卡尔曼滤波
概率机器人第3章主要说明的是高斯滤波的一些理论和应用实例。它们是一种递归状态估计器。所有的高斯技术的核心思想都是：用多元正态分布表示置信度。
$$
p(x)=det(2\pi\Sigma)^{-\frac{1}{2}}exp { -\frac{1}{2}(x-\mu)^T \Sigma^{-1}(x-\mu) }
$$
高斯滤波中的参数：均值和方差都是矩参数。
卡尔曼滤波
卡尔曼滤波主要针对的是线性高斯系统的状态估计问题。根据贝叶斯滤波算法：
$$
\begin{align}
\overline{bel}(X_t) ={} & \int{P(X_t|X_{t-1},U_t)bel(X_{t-1})dX_{t-1}}\\
bel(X_t) ={} & \eta P(Y_t|X_t) \overline{bel}(X_t)
\end{align}
$$
从而获得针对，线性高斯系统的特定的算法。
线性高斯系统
高斯滤波用多元正态分布的核心思想是用多元正态分布表示置信度。并且，卡尔曼滤波针对的是线性高斯系统，所以根据线性高斯系统的状态空间方程：
$$
\begin{align}
X_t ={} & AX_{t-1} + BU_t + \xi_t\\
Y_t ={} & CX_t + \delta_t
\end{align}
$$
由多元分布合成的规则，可以得到
状态转移概率:
$$
p(x_t|u_t,x_{t-1})=det(2 \pi R_t)^{- \frac{1}{2}}exp { -\frac{1}{2}(x_t - A_t x_{t-1} - B_tu_t)^T R_{t}^{-1} (x_t-A_tx_{t-1}-B_tu_t) }
$$
测量概率：
$$
p(z_t|x_t)=det(2 \pi Q_t)^{- \frac{1}{2}}exp { -\frac{1}{2} (z_t - C_t x_t)^T Q_{t}^{-1} (z_t - C_t x_t) }
$$
由于目标系统为线性系统，所以置信度是由其余的多元正态分布进行合成的，因此，置信度 $bel(x_t)$ 也是一个多元正态分布。状态方程与观测方程给出了两个状态变量之间的递推关系，同时，为了满足隐式马尔可夫链的要求，需要知道初始的置信度才能得到各状态的置信度 $bel(x_0)$ 。假设，初始执行度如下表示：
$$
bel(x_0)=p(x_0)=det(2 \pi \Sigma_0)^{- \frac{1}{2}} exp { - \frac{1}{2} (x_0 - \mu_0)^T \Sigma_{0}^{-1}(x_0 - \mu_0 ) }
$$
卡尔曼滤波算法
此处跟《概率机器人》书上写的顺序一样，先给出卡尔曼滤波算法(kalman filter algorithm)。
$$
\begin{align}
\overline{\mu}t ={} & A_t \mu + B_t u_t\\
\overline{\Sigma}t ={} & A_t \Sigma A_t^T + R_t\\
K_t ={} & \overline{\Sigma}_t C_t^T (C_t \overline{\Sigma}_t C_t^T + Q_t )^{-1}\\
\mu_t ={} & \overline{\mu}_t + K_t (z_t - C_t \overline{\mu}_t)\\
\Sigma_t ={} & (I - K_t C_t) \overline{\Sigma}_t
\end{align}
$$
卡尔曼滤波的数学推导
卡尔曼滤波的数学推导过程先省略，后面进行补齐。主要思想是：根据状态空间方程，求出相关量的一阶矩和二阶矩来表示多维正态分布。其中涉及到的相关知识点有：矩阵求逆引理、状态量的完整性、隐式马尔可夫链等3点需要特别进行关注。
扩展卡尔曼滤波
卡尔曼滤波主要针对的是线性高斯情况下使用的算法。如果状态空间是非线性关系的情况下，就要使用到扩展卡尔曼滤波(Extended Kalman Filter,EKF)。
通过泰勒展开的线性化
假设现在的状态空间如下所示：
$$
\begin{align}
x_t ={} & g(u_t,x_{t-1})+ \xi_t\\
z_t ={} & h(x_t) + \delta_t
\end{align}
$$
通过泰勒展开式对非线性函数g和h进行线性化。利用(一阶)泰勒展开的方法，来表示g或h的线性近似函数。
$$
\begin{align}
g(u_t,x_{t-1}) \approx {} & g(u_t,\mu_{t-1})+g'(u_t,\mu_{t-1})(x_{t-1} - \mu_{t-1}) = g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})\\
h(x_t) \approx {} & h(\overline{\mu_t}) + h'(\overline{\mu_t})(x_t - \overline{\mu_t}) = h(\overline{\mu_t}) + H_t (x_t - \overline{\mu_t})
\end{align}
$$
写成高斯形式;
$$
\begin{align}
p(x_t|u_t, x_{t-1}) \approx {} & det(2\pi R_t)^{- \frac{1}{2}}exp { - \frac{1}{2}[x_t - g(u_t, \mu_{t-1}) - G_t(x_t - \mu_{t-1})]^T R^{-1} - [x_t - g(u_t, \mu_{t-1})-G_t(x_{t-1} - \mu_{t-1})] }\\
p(z_t|x_t) \approx {} & det(2\pi Q_t)^{- \frac{1}{2}}exp { - \frac{1}{2}[z_t - h(\overline{\mu_t}) - H_t(x_t - \overline{\mu_t})]^T Q^{-1} - [z_t - h(\overline{\mu_t}) - H_t(x_t - \overline{\mu_t})] }
\end{align}
$$
扩展卡尔曼滤波算法
$$
\begin{align}
\overline{\mu_t} ={} & g(u_t, \mu_{t-1})\\
\overline{\Sigma_t} ={} & G_t \Sigma_{t-1} G_t^T + R_t\\
K_t ={} & \overline{\Sigma}_t H_t^T (H_t \overline{\Sigma}_t H_t^T + Q_t)^{-1}\\
\mu_t ={} & \overline{\mu}_t + K_t (z_t - h(\overline{\mu}_t))\\
\Sigma_t ={} & (I - K_t H_t) \overline{\Sigma}_t
\end{align}
$$
