卡尔曼滤波

观测之间不会相互独立，但是如果建立了隐状态模型，那么观测之间相互独立。

隐马尔可夫模型（Hidden Markov Model）与卡尔曼滤波的关系？

Dynamic Model

	$p(x_t|x_{t-1})$	$p(y_t|x_t)$	$p(x_1)$
Discrete State Dynamic Model (HMM)	$A_{x_{t-1},x_t}$	Any	$\pi$
Linear Gaussian Dynamic Model (KF)	$N(A{x}_{t-1}+B,Q)$	$N(H{x}_t+C,R)$
Non-linear Gaussain Dynamic Model (Particle filter)	$f(x_{t-1})$	$g(y_t)$	$f_0(x_1)$

当前状态是什么？即$p(x_t | y_1, y_2, …, y_t)，也叫filtering。

Kalman Filter

卡尔曼滤波的目的，即解决$p(x_t|y_1,y_2,...,y_t)$。
$$p(x_t|x_{t-1})=N(A{x}_{t-1}+B,Q)$$
等效于
$$x_t=A{x}_{t-1}+B+w,wN(0,Q)$$
同时，
$$p(y_t|x_t)=N(H{x}_t+C,R)$$
等效于
$$y_t=H{x}_t+C+u,uN(0,R)$$
因此，线性高斯模型的参数包括 { A , B , Q , H , C , R } \{A, B, Q, H, C, R\} {A,B,Q,H,C,R}。
$$\begin{array}{rl}{x}_t& =A{x}_{t-1}+B+w,w\sim N(0,Q)\\ y_t& =H{x}_t+C+u,u\sim N(0,R)\end{array}$$
同时，线性高斯模型具有以下三个特性：
$$Cov(X_{t-1},w)=0;Cov(X_t,u)=0;Cov(w,u)=0$$

例子：
假设某个小车的加速度服从高斯分布，$\ddot{x}=a\sim N(0,{\sigma }^2)$，取小车的速度和加速度作为系统模型的状态，
$$X_t=\left\{\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right\}$$
由速度和加速度关系可以得到如下方程组：
$$\begin{array}{rl}{x}_t& =x_{t-1}+{\dot{x}}_{t-1}\Delta t+\frac{1}{2}a(\Delta t{)}^2\\ {\dot{x}}_t& ={\dot{x}}_{t-1}+a\Delta t\end{array}$$
可以推出
$$\left[\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ {\dot{x}}_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}a(\Delta t{)}^2\\ a\Delta t\end{array}\right]$$
令
$$A=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right];B=\left[\begin{array}{c}0\\ 0\end{array}\right];W=\left[\begin{array}{c}\frac{1}{2}a(\Delta t{)}^2\\ a\Delta t\end{array}\right]$$
即系统的状态转移矩阵。
可以得到：
$$X_t=A{X}_{t-1}+W$$
首先，我们计算$X_t$的期望$E[X_t]$，记为$\mu$：
$$\begin{gathered} E[X_t]=E[A{X}_{t-1}+W]=AE[X_{t-1}]+E[W] \\ =A{X}_{t-1}+E\left[\begin{array}{c}\frac{1}{2}a(\Delta t{)}^2\\ a\Delta t\end{array}\right]=A{X}_{t-1} \end{gathered}$$
接着，计算$X_t$的协方差
$$\begin{gathered} E[(X_t-\mu )(X_t-\mu {)}^T]=E[(A{X}_{t-1}+W-A{X}_{t-1})(A{X}_{t-1}+W-A{X}_{t-1}{)}^T] \\ =E[W{W}^T]=E\left[\left[\begin{array}{c}\frac{1}{2}a(\Delta t{)}^2\\ a\Delta t\end{array}\right]\left[\begin{array}{cc}\frac{1}{2}a(\Delta t{)}^2& a\Delta t\end{array}\right]\right] \\ =E\left[\begin{array}{cc}\frac{1}{4}{a}^2\Delta t^4& \frac{1}{2}{a}^2\Delta t^3\\ \frac{1}{2}{a}^2\Delta t^3& a^2\Delta t^2\end{array}\right]=E[a^2]\left[\begin{array}{cc}\frac{1}{4}\Delta t^4& \frac{1}{2}\Delta t^3\\ \frac{1}{2}\Delta t^3& \Delta t^2\end{array}\right] \end{gathered}$$
因为$a\sim N(0,{\sigma }^2)$，即均值$u_a=0$，所以 $E[a^2]=E[(a-u_a{)}^2]$，也就是a的方差${\sigma }^2$。
由此可得，$X_t$的协方差为
$$E[(X_t-\mu )(X_t-\mu {)}^T]={\sigma }^2\left[\begin{array}{cc}\frac{1}{4}\Delta t^4& \frac{1}{2}\Delta t^3\\ \frac{1}{2}\Delta t^3& \Delta t^2\end{array}\right]$$
上式也就是过程噪声W的协方差矩阵Q。至此，已得到该线性高斯系统的参数 { A , B , W } \{A, B, W\} {A,B,W}。

考虑到该例子中，所观测到的小车状态仅为小车位置，即$x_t$，易得该线性高斯系统的观测方程：
$$y_t=x_t+U⟹y_t=\left[\begin{array}{c}1,0\end{array}\right]X_t+U$$
令
$$H=\left[\begin{array}{c}1,0\end{array}\right];C=[0];$$

推导过程

上节最开始提到了，KF的主要目的就是估计$p(x_t|y_1,y_2,...,y_t)$。
而
$$\begin{array}{rl}& p(x_t|y_1,y_2,...,y_t)\propto p(x_t,y_1,y_2,...,y_t)\\ & =p(y_t|x_t,y_1,y_2,...,y_{t-1})p(x_t|y_1,y_2,...,y_{t-1})p(y_1,y_2,...,y_{t-1})\\ & \propto p(y_t|x_t)p(x_t|y_1,y_2,...,y_{t-1})\end{array}$$
其中，$p(x_t|y_1,y_2,...,y_{t-1})$ 被称为预测阶段，$p(x_t|y_1,y_2,...,y_t)$被称为更新阶段。
对于预测阶段来说：
$$\begin{array}{rl}& p(x_t|y_1,y_2,...,y_{t-1})={\int }_{x_{t-1}}p(x_t,x_{t-1}|y_1,y_2,...,y_{t-1})d{x}_{t-1}\\ & ={\int }_{x_{t-1}}p(x_t|x_{t-1},y_1,y_2,...,y_{t-1})p(x_{t-1}|y_1,y_2,...,y_{t-1})p(y_1,y_2,...,y_{t-1})\\ & \propto {\int }_{x_{t-1}}p(x_t|x_{t-1},y_1,y_2,...,y_{t-1})p(x_{t-1}|y_1,y_2,...,y_{t-1})\\ & ={\int }_{x_{t-1}}p(x_t|x_{t-1})p(x_{t-1}|y_1,y_2,...,y_{t-1})\end{array}$$
上式中$p(x_{t-1}|y_1,y_2,...,y_{t-1})$即为上一步更新结果，那么就可以通过recursive方式进行计算。
注意，对于预测和更新两个阶段来说，他们的概率都服从正态分布，也就是：
$$\begin{array}{rl}Previousupdate:& p(x_{t-1}|y_1,y_2,...,y_{t-1})\sim N({\hat{\mu }}_{t-1},{\hat{\Sigma }}_{t-1})\\ Prediction:& p(x_t|y_1,y_2,...,y_{t-1})\sim N({\bar{\mu }}_t,{\bar{\Sigma }}_t)\\ Update:& p(x_t|y_1,y_2,...,y_t)\sim N({\hat{\mu }}_t,{\hat{\Sigma }}_t)\end{array}$$
上式仅描述了t时刻的状态概率分布，但是如何估计或者修正t时刻的实际状态，需要换另一种形式对上式进行改写。
$$\begin{array}{rl}p(x_{t-1}|y_1,y_2,...,y_{t-1})& \sim N({\hat{\mu }}_{t-1},{\hat{\Sigma }}_{t-1})\\ x_{t-1}|y_1,y_2,...,y_{t-1}& ={\hat{\mu }}_{t-1}+\Delta x_{t-1},\Delta x_{t-1}\sim N(0,{\hat{\Sigma }}_{t-1})\\ & =E[x_{t-1}]+\Delta x_{t-1}\end{array}$$
$$\begin{array}{rl}p(x_t|y_1,y_2,...,y_{t-1})& \sim N({\bar{\mu }}_t,{\bar{\Sigma }}_t)\\ x_t|y_1,y_2,...,y_{t-1}& ={\bar{\mu }}_t+\Delta x_t,\Delta x_t\sim N(0,{\bar{\Sigma }}_t)\\ & =E[x_t]+\Delta x_t\\ x_t|y_1,y_2,...,y_{t-1}& =A{x}_{t-1}+w\\ & =A(E[x_{t-1}]+\Delta x_{t-1})+w\\ & =AE[x_{t-1}]+A\Delta x_{t-1}+w\\ p(x_t|y_1,y_2,...,y_{t-1})& =N(AE[x_{t-1}],E[(\Delta x_t)(\Delta x_t{)}^T])\end{array}$$
由于 $E[x_t]=AE[x_{t-1}]$，所以可知$\Delta x_t=A\Delta x_{t-1}+w$。
$$\begin{array}{rl}{y}_t|y_1,y_2,...,y_{t-1}& =H{x}_t+v\\ & =H(AE[x_{t-1}]+A\Delta x_{t-1}+w)+v\\ & =HAE[x_{t-1}]+HA\Delta x_{t-1}+Hw+v\\ & =E[y_t]+\Delta y_t\\ p(y_t|y_1,y_2,...,y_{t-1})& =N(HAE[x_{t-1}],E[(\Delta y_t)(\Delta y_t{)}^T])\end{array}$$