第3章 线性高斯系统状态估计
线性高斯系统状态估计问题可以分为连续时间状态估计和离散时间状态估计。连续时间的状态估计又称为高斯过程回归(Guassian Process Regression)。离散时间状态估计可以看作连续时间状态估计问题在观测时刻的精确实现。先从离散时间的状态估计问题讲起。
离散时间状态估计问题
离散时间状态估计用运动方程和观测方程来建模：
$$\begin{split}
x_k&=&A_{k-1}x_{k-1}A_{k-1}^T+v_k+w_k\\
y_K&=&C_kx_k+n_k\\
x_0 &\sim& \mathcal{N}(\check{x}_0, \check{P}_0) \\
v_k &\sim &\mathcal{N}(0,Q_k) \\
n_k& \sim &\mathcal{N}(0,R_k)
\end{split}$$
离散时间状态估计的解决方法可以分为批量的方法和迭代的方法。后者可以看做前者的高效实现。
批量处理
批量的方法分为两种范式：最大后验、贝叶斯推断。在线性高斯的情况下二者是等价的。贝叶斯推断中，系统状态的估计就是概率密度函数中的均值，因为线性高斯的假设下后验概率函数也是一个高斯函数，它的均值就是在最大后验概率密度处，所以它与最大后验的方法是等价的。
最大后验方法
最大后验方法是求解
$$
\mathop{\arg\max}_{x} p(x|v,y)
$$
利用贝叶斯公式
$$
\mathop{\arg\max}_{x} p(x|v,y) = \mathop{\arg\max}_{x}p(y|x)p(x|v)
$$
而
$$
p(y|x)=\prod_{k=0}^Kp(y_k|x_k) \\
p(x|v)=p(x_0|\check{x}_0)\prod_{k=1}^Kp(x_k|x_{k-1},v
)$$
对$p(y|x)p(x|v)$取对数，去掉与$x$无关的项可以构建代价函数$\boldsymbol{J}(x) = \sum_{k=0}^K J_{v,k}(x)+J_{y,k}(x)$。其中$ J_{v,k}(x)$和$J_{y,k}(x)$都是马哈拉诺比斯距离，是高斯概率密度函数中指数项的指数。
将代价函数\(\boldsymbol{J}(x)\)改写为矩阵形式可得：
\[\boldsymbol{J}(x)=\frac{1}{2}(z-Hx)^TW^{-1}(z-Hx)
\]
其中：
\[z=\left [ \begin {array} {rcl}
v \\
y
\end{array} \right ], H=\left[ \begin {array}{rcl}
A^{-1}\\
C
\end{array}\right], W= \left[ \begin{matrix}
Q & \\
& R
\end{matrix}\right] \]
\[A^{-1}=\left[ \begin{matrix}
1 & & & & & \\
-A_0& 1 & & & & \\
& -A_1 &1 & & & \\
& &-A_2 & 1 & & \\
& & & \ddots &\ddots & \\
& & & & -A_K & 1
\end{matrix}\right]
\]
对\(\boldsymbol{J}(x)\)求导，并令导数为零可以得到线性方程组：
\[\frac{\partial \boldsymbol{J}(x)}{\partial x^T}\big| \hat{x}=-H^TW^{-1}(z-Hx) = 0 \\ \Longrightarrow H^TW^{-1}H \hat{x} = H^TW^{-1}z
\]
求解该线性方程组得到\(\hat{x}\)就是系统状态\(x\)的最大后验估计。
需要注意的是求解上述方程组，并不直接求\(H^TW^{-1}H\)的逆。由运动方程和观测方程的结构可以知道\(H^TW^{-1}H\)是一个稀疏矩阵，有比直接求逆更高效的解法，这就是后面的迭代求解方法。
贝叶斯推断方法
贝叶斯推断的方法首先由运动方程、初始状态和系统输入得到一个先验概率密度，然后用观测值更新先验概率密度得到后验概率密度
把运动方程和观测方程写成矩阵的形式：
\[\begin{split}
x&=& A(v+w) \\
y &=& Cx+n \\
(v+w) &\sim& \mathcal{N}(v,Q)
\end{split}\]
高斯随机变量通过线性系统\(A\)可得先验概率密度函数：
\[p(x|v) = \mathcal{N}(\check{x}, \check{P})=\mathcal{N}(Av, AQA^T)
\]
然后再考虑观测值。求\(x,y\)的联合概率密度函数：
\[p(x,y|v)=\mathcal{N}\Bigg( \left[ \begin{array} {rcl}
\check{x} \\
C\check{x}
\end{array} \right],
\left[ \begin{array} {rcl}
\check{P}& \check{P}C^T \\
C\check{P}& C\check{P}C^T+R\end{array} \right]\Bigg)\]
利用第2章中介绍的联合概率密度求解条件概率密度函数的公式，并进行一些恒等变换，可以得到：
\[p(x|v,y)=\mathcal{N}\Big( \underbrace{(\check{P}^{-1}+C^TR^{-1}C)^{-1}(\check{P}^{-1}\check{x}+C^TR^{-1}y)}_{\hat{x},mean}, \underbrace{(\check{p}^{-1}+C^TR^{-1}C)^{-1}}_{\hat{P},covariance}\Big)
\]
这就是贝叶斯推断得到的完全的概率密度函数。
为什么说它与最大后验方法是等价的？
\[\hat{x}=(\check{P}^{-1}+C^TR^{-1}C)^{-1}(\check{P}^{-1}\check{x}+C^TR^{-1}y) \\
\Longrightarrow (\check{P}^{-1}+C^TR^{-1}C)\hat{x}=\check{P}^{-1}\check{x}+C^TR^{-1}y
\]
将\(\check{x}=Av\)和\(\check{P}=AQA^T\)代入上式可得：
\[(A^{-T}Q^{-1}A^{-1}+C^TR^{-1}C)\hat{x}=A^{-T}Q^{-1}v+C^TR^{-1}y
\]
令
\[z=\left[ \begin{array}{rcl}
v \\
y
\end{array} \right],
H=\left[ \begin{array}{rcl}
A^{-1}\\
C
\end{array}\right],
W=\left[ \begin{array}{rcl}
Q& \\
&R
\end{array} \right] \]
可得：
\[H^TW^{-1}H\hat{x}=H^TW^{-1}z
\]
迭代的方法
依据估计当前状态是否使用未来时刻的数据，迭代的方法可以分为Smoother和Filter。Smoother估计每个时刻的状态都使用全部时刻的数据（包括未来时刻的），它与批处理的方式是等价的，可以看作批处理方式的一种高效实现，是\(H^TW^{-1}H\hat{x}=H^WW^{-1}z\)的快速求解方法。但由于它不符合因果律，因此不能online的处理。Filter估计当前状态时只使用当前时刻和之前时刻的数据，它是符合因果律的，可以online的处理，它与Smoother的前向迭代是等价的。经典的状态估计Filter是Kalman Filter。
Smoother
书中介绍了两种Smoother， Cholesky Smoother 和RTS Smoother。 Cholesky Smoother通过对\(H^TW^{-1}H\)进行sparse Cholesky分解推导出来。 RTS Smoother方程是Smoother的经典形式， 它与Cholesky Smoother在代数上是等价的，也就是说可以通过代数变换互相推导出来。书中先推导出Cholesky Smoother的迭代算法，然后通过对其中的迭代变量\(I_k, q_k\)进行代数变换推导出经典的RTS Smoother形式。RTS Smoother的前向迭代过程就是Kalman Filter。
Cholesky Smoother
\(H^TW^{-1}H\)具有如下的稀疏形式
\[H^TW^{-1}H = \left[ \begin{matrix}
* & * & & & & \\
* & * & * & & & \\
& * & * & * & & \\
& & \ddots& \ddots & \ddots & \\
& & & *&*& * \\
& & & & * & *
\end{matrix}\right]\]
可以高效的对它进行Cholesky分解
\[H^TW^{-1}H = LL^T \tag{1}
\]
其中：
\[L = \left[\begin{matrix}
* & \\
* & * \\
& *& * \\
& & \ddots & \ddots \\
& & & * &* \\
& & & & *& *
\end{matrix}\right]\]
这样\(H^TW^{-1}A\hat{x}=H^TW^{-1}z\)可以转换为\(Ld=H^TW^{-1}z\)的形式，\(d= L^T\hat{x}\)
由于\(L\)是一个块下三角阵，并且只有沿着对角线的两个斜列不为零，因此可以简单的通过前向迭代代入的方式求得全部的\(d\)。\(L^T\)是一个类似的块上三角阵，求得\(d\)之后，通过后向迭代代入可以求得\(\hat{x}\)。
具体来说， 令
\[L=\left[ \begin{matrix}
L_0 & \\
L_{1,0} & L_1 &\\
& L_{2,1} & L_2 &\\
& &\ddots &\ddots & \\
& & & L_{K-1,K-2} & L_{K-1} \\
& & & & L_{K,K-1} & L_{K}
\end{matrix}\right]\]
代入公式\((1)\)，并作分块矩阵的运算可以得到：
\[\begin{eqnarray*}
L_0L_0^T&= &\underbrace{P_0^{-1}+C_0^TR_0^{-1}C_0}_{I_0}+A_0^TQ_1^{-1}A_0 \tag{2.1}\\
L_{10}L_0^T&=&-Q_1^{-1}A_0\tag{2.2}\\
L_1L_1^T&=&\underbrace{-L_{10}L_{10}^T + Q_1^{-1}+C_1^TR_1^{-1}C_1}_{I_1}+A_1^TQ_2^{-1}A_2\tag{2.3}\\
L_{20}L_1^T&=&-Q_2^{-1}A_1\tag{2.4}\\
&\vdots&\\
L_{K-1}L_{K-1}^T&=&\underbrace{-L_{K-1,K-2}L_{K-1,K-2}^T+Q_{K-1}^{-1}+C_{K-1}^TR_{K-1}C_{K-1}}_{I_{K-1}}+A_{K-1}^TQ_K^{-1}A_{K-1}\tag{2.5}\\
L_{K,K-1}L_{K-1}^T&=&-Q_K^{-1}A_{K-1}\tag{2.6}\\
L_KL_K^T &=&\underbrace{-L_{K,K-1}L_{K,K-1}^T+Q_K^{-1}+A_K^TR_K^{-1}A_K}_{I_K}\tag{2.7}
\end{eqnarray*}
\]
由公式\((2.*)\)可以看到矩阵\(L\)的每个子块矩阵都可以表达为某个信息矩阵（协方差矩阵的逆）的平方根的形式，即\(L_*L_*^T=I_*\)，因此说Cholesky Smoother是一种square-root information smoother。
由公式\((2.1)\)用一个小规模的稠密Cholesky分解可以求得\(L_0\)，代入式\((2.2)\)可以求得\(L_{10}\)，再将\(L_{10}\)代入\((2.3)\)可以求得\(L_1\)，一直迭代下去，可以求得矩阵\(L\)的所有子块。
得到\(L\)之后就可以求解\(Ld=H^TW^{-1}z\)，再次做分块运算，可以得到：
\[\begin{eqnarray*}
L_0d_0 &=& \underbrace{\check{P}_0^{-1}\check{x}_0+C^TR_0^{-1}y_0}_{q_0}-A_0^TQ_1^{-1}v_1\tag{3.1}\\
L_1d_1 &=& \underbrace{-L_{10}d_0+Q_1^{-1}v_1+C_1R_1^{-1}y_1}_{q_1}-A_1^TQ_2^{-1}v_2\tag{3.2}\\
&\vdots& \\
L_{K-1}d_{K-1}&=&\underbrace{-L_{K-1,K-2}d_{K-2}+Q_{K-1}^{-1}v_{K-1}+C_{K-1}^TR_{K-1}y_{K-1}}_{q_{K-1}}-A_{K-1}^TQ_K^{-1}v_K\tag{3.3}\\
L_Kd_K &=& \underbrace{-L_{K,K-1}d_{K-1}+Q_K^{-1}v_K+C_K^TR_K^{-1}y_K}_{q_K}\tag{3.4}
\end{eqnarray*}
\]
由式\((3.1)\)可以求得\(d_0\)，代入式\((3.2)\)可以求得\(d_1\)，持续迭代下去，可以求得整个\(d\)。有了\(d\)之后，最后一步就是求解\(L^T\hat{x}=d\)。由于\(L^T\)是一个块上三角阵，其最后一个块行只有一个非零块，展开从最后一个块行开始，可以得到：
\[\begin{eqnarray*}
L_K^T\hat{x}_K&=&d_K\tag{4.1}\\
L_{K-1}^T\hat{x}_{K-1}&=&-L_{K,K-1}^T\hat{x}_K+d_{K-1}\tag{4.2}\\
&\vdots&\\
L_1^T\hat{x}_1&=&-L_{21}^T\hat{x}_2+d_1\tag{4.3}\\
L_0^T\hat{x}_0&=&-L_{10}^T\hat{x}_1+d_0\tag{4.4}
\end{eqnarray*}\]
由式\((4.1)\)可求得\(\hat{x}_K\)，代入式\((4.2)\)可求得\(\hat{x}_{K-1}\)，一直迭代代入，可以求得整个\(\hat{x}\)。
按照时间戳的顺序整理公式\((2)(3)(4)\)可以得到Cholesky Smoother的迭代算法如下：
\[\begin{eqnarray*}
forward:\\
(k=1\cdots K)\\
L_{k-1}K_{k-1}^T&=&I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1} \tag{5.1}\\
L_{k-1}d_{k-1}&=&q_{k-1}-A_{k-1}^TQ_k^{-1}v_k\tag{5.2}\\
L_{k,k-1}L_{k-1}^T&=&-Q_k^{-1}A_{k-1}\tag{5.3}\\
I_k&=&-L_{k,k-1}L_{k,k-1}^T+Q_k^{-1}+C_k^TR_k^{-1}C_k\tag{5.4}\\
q_k&=&-L_{k,k-1}d_{k-1}+Q_k^{-1}v_k+C_k^TR_k^{-1}y_k\tag{5.5}\\
backward:\\
(k=K\cdots 1)\\
L_{k-1}^T\hat{x}_{k-1} &=&-L_{k,k-1}^T\hat{x}_k+d_{k-1}\tag{5.6}
\end{eqnarray*}\]
初始状态为：
\[\begin{eqnarray*}
I_0&=&\check{P}_0^{-1}+C_0^TR_0^{-1}C_0\\
q_0&=&\check{P}_0^{-1}x_0 +C_0^TR_0^{-1}y_0\\
\hat{x}_K&=&L_K^{-T}d_K
\end{eqnarray*}\]
该算法的前向过程，每次迭代都将\(I_k,q_k\)向前推进一个时刻，在推进的同时，求解出了\(L\)和\(d\)的相应的项。后向迭代的时候会使用相应的\(L\)和\(d\)的项。前向迭代的过程中似乎没有求的\(x, P\)的值，但他们其实是隐藏在\(I,q\)中的。从后面的RTS Smoother中可以看到\(I_k=P_k^{-1}, P_k^{-1}x_k=q_k\)。
RTS Smoother
RTS Smoother是smoother的经典形式， 它与Cholesky Smoother在代数上是完全等价的，可以从Cholesky Smoother的前向迭代量\(I_k,q_k\)开始推导，先看\(I_k\)：
由\((5.3)\)求解\(L_{k,k-1}\)代入\((5.1)\)然后再代入\((5.4)\)，经过变换可以得到
\[I_k=(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1} +C_k^TR_k^{-1}C_k
\]
令\(\hat{P}_{k,f}=I_k^{-1}\)，代表前向过程中k时刻系统协方差矩阵的后验。
令
\[\check{P}_{k,f}= A_{k-1}\hat{P}_{k-1,f}A_{k-1}^T+Q_k\tag{6}
\]
代表向前过程中k时刻系统协方差的先验。
所以有：
\[\hat{P}_{k,f}^{-1} = \check{P}_{k,f}^{-1}+C_k^TR_k^{-1}C_k
\]
上式是信息矩阵（协方差矩阵的逆）的形式，我们改写一下，写成协方差矩阵的形式。定义卡尔曼增益矩阵(Kalman Gain Matrix)
\[K_k=\hat{P}_{k,f}C^TR_k^{-1}=\check{P}_{k,f}C_k^T(C_k\check{P}_{k,f}C_k^T+R_k)^{-1}
\]
这样就有
\[\hat{P}_{k,f}=(1-K_kC_k)\check{P}_{k,f}\tag{7}
\]
公式\((6)(7)\)联合起来表达的意思很明确，先由运动方程得到当前状态协方差矩阵的先验，再由观测方程对先验进行修正得到后验。
接下来由\(q_k\)推导出当前时刻系统状态的数学期望，也就是后验估计。需要注意的是这个后验估计跟前面的协方差矩阵的后验估计一样，仅仅是前向过程推导的后验估计，而不是整个smoother得到的后验估计，它记作\(\hat{x}_{k,f}\)，\(f\)表示前向过程。
由式\((5.3),(5.2)\)分别求的\(L_{k,k-1},d_k\)代入\((5.1)\)再代入\((5.5)\)，经过变换可以得到\(q_k\)由\(q_{k-1}\)表示的递推公式：
\[q_k=(A_{k-1}I_{k-1}A_{k-1}^T+Q_k)^{-1}A_{k-1}I_{k-1}^{-1}q_{k-1}+(A_{k-1}I_{k-1}^{-1}A_{k-1}^T+Q_k)^{-1}v_k+C_kR_k^{-1}y_k \\
\Longrightarrow q_k = \check{P}_{k,f}A_{k-1}\hat{P}_{k-1,f}q_{k-1}+\check{P}_{k,f}v_k+C_k^TR_k^{-1}y_k\\
\Longrightarrow q_k=\check{P}_{k,f}(A_{k-1}\hat{P}_{k-1,f}q_{k-1}+v_k)+C_k^TR_k^{-1}y_k\]
令\(\hat{P}_{k,f}^{-1}\hat{x}_{k,f}=q_k\)可得：
\[\begin{eqnarray*}
\check{x}_{k,f} &=& A_{k-1}\hat{x}_{k-1}+v_k \tag{8.1}\\
\hat{P}_{k,f}^{-1}\hat{x}_{k,f}&=&\check{P}_{k,f}^{-1}\check{x}_{k,f}+C_k^TR_k^{-1}y_k \tag{8.2}
\end{eqnarray*}\]
由\((8.2)\)可得：
\[\hat{x}_{k,f}=\underbrace{\hat{P}_{k,f}\check{P}_{k,f}^{-1}}_{1-K_kC_k}\check{x}_{k,f}+\underbrace{\hat{P}_{k,f}C_k^TR_k^{-1}}_{K_k}y_k=\check{x}_{k,f}+K_k(y_k-C_k\check{x}_{k,f}) \tag{9}
\]
公式\((8.1)(9)\)联合起来跟前面的协方差矩阵类似，先由运动方程得到数学期望的先验，然后由观测方程对先验进行修正得到后验。
到这里前向过程的推导就完毕了。接下来推导后向过程。
与前面类似，将\((5.1)(5.2)(5.3)\)代入到\((5.6)\)可以得到：
\[\begin{split}\hat{x}_{k-1}&=&(L_{k-1}L_{k-1^T})^{-1}L_{k-1}(-L_{k,k-1}^T\hat{x}_k+d_{k-1}) \\
&=&I_{k-1}^{-1}A_{k-1}^T(A_{k-1}I_{k-1}^{-1}A_{k-1}+Q_k)^{-1}(\hat{x_k}-v_k)+(I_{k-1}+A_{k-1}^TQ_k^{-1}A_{k-1})^{-1}\\
& = & \hat{x}_{k-1,f}+\hat{P}_{k-1}A_{k-1}^T\check{P}_{k,f}^{-1}(\hat{x}_k-\check{x}_{k,f})
\end{split}\tag{10}\]
综合\((6)\sim (10)\)可以得到RTS Smoother的迭代算法：
\[\begin{eqnarray*}
forward:\\
(k=1\cdots K)\\
\check{P}_{k,f} &=& A_{k-1}\hat{P}_kA_{k-1}^T+Q_k \\
\check{x}_{k,f}&=&A_{k-1}\hat{x}_{k-1} +v_k\\
K_k &=& \check{P}_{k,f}C_k^T(C_k\check{P}_{k,f}C_k^T+R_k)^{-1}\\
\hat{P}_{k,f}&=&(1-K_kC_k)\check{P}_{k,f}\\
\hat{x}_{k,f}&=&\check{x}_{k,f}+K_k(y_k-C_k\check{x}_{k,f})\\
backward:\\
(k=K\cdots 1)\\
\hat{x}_{k-1}&=&\hat{x}_{k-1,f}+\hat{P}_{k-1,f}A_{k-1}^T\check{P}_{k,f}(\hat{x}_k-\check{x}_{k,f})
\end{eqnarray*}\]
初始值为
\[\begin{eqnarray*}
\check{P}_{0,f}&=&\check{P}_0\\
\check{x}_{0,f}&=&\check(x)_0\\
\hat{x}_K&=&\hat{x}{K,f}
\end{eqnarray*}\]
RTS Smoother的前向过程就是Kalman Filter。由推导的过程可以知道RTS Smoother的前向过程完全由Cholesky Smoother的前向过程变换得到的，因此Kalman Filter与Cholesky Smoother的前向过程是完全等价的。迭代变量之间的关系为\(I_k=\hat{P}_{k,f}^{-1}, q_k = \hat{P}_{k,f}^{-1}\hat{x}_{k,f}\)。
Kalman Filter
Kalman Filter除了可以从RTS Smoother的前向过程得到，也有其它方式推导得到。书中介绍了另外三种Kalman Filter的推导方法：
- 从最大后验概率的角度推导
- 从贝叶斯推断的角度推导
- 从最优增益矩阵的角度推导
这部分开始之前书上先介绍了一种将批量处理方式分解为一个前向过程和一个后向过程的方法。之前smoother中也讲了这样一种分解方法：前向过程得到系统状态的一个估计，后向过程对这个估计进行修正。本节中的分解方法有些不同，后向过程并不是对前向过程进行修正，而是仅使用当前时刻之后的数据对当前时刻的状态进行估计。然后前向估计和后向估计依据2.2.6节Normalize Production的方法综合起来得到最终的估计：
\[\hat{P}_k^{-1} = \hat{P}_{k,f}^{-1}+\hat{P}_{k,b}^{-1}\\
\hat{P}_k^{-1}\hat{x}_k=\hat{P}_{k,f}^{-1}\hat{x}_{k,f}+\hat{P}_{k,b}^{-1}\hat{x}_{k,b}
\]
这个分解方法跟smoother中的分解方法差别主要是在后向过程，但KF本身只涉及到前向过程，在这里介绍这种分解方法感觉逻辑上不太合理。一直没有搞明白它与后面三节有什么关系。但是不管怎么样，还是先把这种分解方式讲一下。
这种分解方法的思路是，首先由\(p(x|v,y)\)得到当前时刻状态的概率密度函数\(p(x_k|v,y)\)
\[p(x_k|v,y)=\int_{\forall i, i\neq k}p(x_0,\cdots x_K|v,y)dx_{i,\forall i\neq k}
\]
然后将\(p(x_k|v,y)\)分解为两部分，一部分只与当前时刻及之前时刻的数据有关，另一部分只与之后时刻的数据有关：
\[p(x_k|v,y)=\eta p(x_k|\check{x}_0,v_{1:k},y_{0:k})p(x_k|v_{k+1:K},y_{k+1:K}) \tag{11}
\]
前面批量方法中我们已经有\(H^TW^{-1}H\hat{x}=H^TW^{-1}z， \hat{x}=(H^TW^{-1}H)^{-1}H^TW^{-1}z, \hat{P}=(H^TW^{-1}H)^{-1}\)，为了得到上面分解的两个概率密度。需要对\(z,H,W\)重新调整，按照时间戳的顺序重新排列，排列之后\(x\)顺序不变，\(H^TW^{-1}H\hat{x}=H^TW^{-1}z\)依然成立。排列后：
\[z=\left[\begin{array}{rcl}
\check{x}_0\\
y_0\\
v_1\\
y_1\\
\vdots\\
v_K\\
y_K
\end{array}\right],
W=\left[ \begin{matrix}
\check{P}_0 &\\
& R0\\
&&Q_1\\
&&&R_1\\
&&&&\ddots\\
&&&&&Q_K\\
&&&&&&R_K
\end{matrix}\right],
H=\left[\begin{matrix}
1\\
C_0\\
-A_0 &1\\
& C_1\\
& -A_1 &1\\
&&C_2\\
&&\ddots&\ddots\\
&&&-A_{K-1}&1\\
&&&&C_K
\end{matrix}\right]
\]
重新排列之后对\(H\)进行分块
\[H=\begin{bmatrix}
H_{11}\\
H_{12}&H_{22}\\
&H_{32}&H_{33}\\
&&H_{43}
\end{bmatrix}
\]
相应的\(z,W\)也进行分块，然后代入\(H^TW^{-1}H\hat{x}=H^TW^{-1}z\)整理之后，\(H^TW^{-1}H\hat{x}=H^TW^{-1}z\)就变成了：
\[\begin{bmatrix}
L_{11} &L_{12}\\
L_{12}^T&L_{22}&L_{32}^T\\
&L_{32}&L_{33}
\end{bmatrix}
\begin{bmatrix}
\hat{x}_{0:k-1}\\
\hat{x}_k\\
\hat{x}_{k+1:K}
\end{bmatrix}=
\begin{bmatrix}
r_1\\
r_2\\
r_3
\end{bmatrix}
\]
进行块行变换求解\(\hat{x}_k\):
\[\underbrace{L_{22}-L_{12}^TL_{11}^{-1}L_{12}-L_{32}^TL_{33}^{-1}L_{32}}_{\hat{P}_k^{-1}}\hat{x}_k=\underbrace{r_2-L_{12}^TL_{11}^{-1}r_1 -L_{32}^TL_{33}^{-1}r_3}_{q_k}
\]
将\(L_*, r_*\)用分块矩阵的子块代入，整理， 并定义\(\hat{P}_{k,f},\hat{P}_{k,b},q_{k,f},q_{k,b}\)得到：
\[\begin{eqnarray*}
\hat{P}_k^{-1}&=&H_{22}^T\hat{P}_{k,f}H_{22}+H_{32}^T\hat{P}_{k,b}H_{32}\\
q_k&=&H_{22}^Tq_{k,f}+H_{32}^Tq_{k,b}
\end{eqnarray*}
\]
接下来，可以定义前向和后向两个estimator:
\[\begin{eqnarray*}
\hat{P}_{k,f}^{-1}\hat{x}_{k,f}&=&q_{k,f}\\
\hat{P}_{k,b}^{-1}\hat{x}_{k,b}&=&q_{k,b}
\end{eqnarray*}
\]
这里\(\hat{x}_{k,f}\)只依赖于当前时刻及之前的数据，\(\hat{x}_{k,b}\)只依赖于当前时刻之后的数据。综合起来就有：
\[\begin{eqnarray*}
\hat{P}_k^{-1}&=&H_{22}^T\hat{P}_{k,f}H_{22}+H_{32}^T\hat{P}_{k,b}H_{32}\\
\hat{x}_k&=& \hat{P}_k^{-1}(H_{22}^T\hat{P}_{k,f}^{-1}\hat{x}_{k,f}+H_{32}^T\hat{P}_{k,b}^{-1}\hat{x}_{k,b})
\end{eqnarray*}
\]
参考公式\((11)\)我们就有：
\[\begin{eqnarray*}
p(x_k|v,y) &\sim & \mathcal{N}(\hat{x}_k,\hat{P}_k)\\
p(x_k|\check{x}_0,v_{1:k},y_{0:k}) & \sim& \mathcal{N}(\hat{x}_{k,f},\hat{P}_{k,f})\\
p(x_k|v_{k+1:K},y_{k+1:K}) & \sim& \mathcal{N}(\hat{x}_{k,b},\hat{P}_{k,b})
\end{eqnarray*}
\]
最大后验概率推导KF
该方法中有一个基本的假设就是马尔科夫假设，在线性高斯系统中，该假设是成立的，这个从运动方程和观测方程可以很容易的看到。要得到迭代的KF算法，先假设\(x_{k-1}\)时刻的状态已经估计出来为\(\{\hat{x}_{k-1},\hat{P}_{k-1}\}\),现在由\(k-1\)时刻的状态推导\(k\)时刻的状态。我们由前面的批量方法中已经知道\(H^TW^{-1}H\hat{x}=H^TW^{-1}z\)。
现在定义
\[z=\begin{bmatrix}
\hat{x}_{k-1}\\v_k\\y_k
\end{bmatrix},
H=\begin{bmatrix}
1\\
-A_{k-1}& 1\\
&C_k
\end{bmatrix},
W=\begin{bmatrix}
\hat{P}_{k-1} \\
&Q_k\\
&&R_k
\end{bmatrix},
\hat{x}=\begin{bmatrix}
\hat{x}_{k-1}^{'}\\
\hat{x}_k
\end{bmatrix}
\]
需要注意的是这里的\(\hat{x}_{k-1}^{'}\neq \hat{x}_{k-1}\)，\(\hat{x}_{k-1}^{'}\)是用\(k\)时刻数据通过后向过程修正过的系统状态。这里的\(\hat{x}_{k-1}\)其实是前面batch方法的\(\hat{x}_{k-1,f}\)，由于这里推导KF，不考虑后向过程，为了简单简写成了\(\hat{x}_{k-1}\)，不要跟前面batch方法的混淆了。\(\hat{x}_{k-1}^{'}\)与前面batch方法的\(\hat{x}_{k-1}\)相似，但也是不相等的，\(\hat{x}_{k-1}^{'}\)仅仅被\(k\)时刻的数据后向修正过，前面的\(\hat{x}_{k-1}\)被\(k-1\)时刻之后的所有数据修正过（迭代进行的，参考RTS Smoother的后向过程）。
将\(z,\hat{x},H,W\)代入到\(H^TW^{-1}H\hat{x}=H^TW^{-1}z\)整理得到：
\[\begin{bmatrix}
\hat{P}_{k-1}^{-1}+A_{k-1}^TQ_k^{-1}A_{k-1} & -A_{k-1}^TQ_k^{-1}\\
-Q_k^{-1}A_{k-1} & A_k^{-1}+C_k^TR_k^{-1}C_k
\end{bmatrix}
\begin{bmatrix}
\hat{x}_{k-1}^{''}\\
\hat{x}_k
\end{bmatrix}=
\begin{bmatrix}
\hat{P}_{k-1}^{-1}\hat{x}_{k-1-A_{k-1}^T}Q_k^{-1}v_k\\
Q_k^{-1}v_k+C_k^TR_k^{-1}y_k
\end{bmatrix}
\]
我们不关心\(\hat{x}_{k-1}^{'}\)，对上式进行块行变换，可以得到：
\[\hat{P}_k^{-1}\hat{x}_k=\check{P}_k^{-1}\check{x}_k+C_kR_k^{-1}y_k
\]
其中：
\[\begin{eqnarray*}
\check{P}_k &=& Q_k +A_{k-1}\hat{P}_{k-1} A_{k-1}^T\\
\hat{P}_k^{-1} &=& (\check{P}_k+C_k^TR_k^{-1}C_k)^{-1}\\
\check{x}_k &=&A_{k-1}\hat{x}_{k-1}+v_k
\end{eqnarray*}
\]
联合起来就得到了KF的信息矩阵形式：
\[\begin{eqnarray*}
\check{P}_k &=& Q_k +A_{k-1}\hat{P}_{k-1} A_{k-1}^T\\
\check{x}_k &=&A_{k-1}\hat{x}_{k-1}+v_k\\
\hat{P}_k^{-1} &=& (\check{P}_k+C_k^TR_k^{-1}C_k)^{-1}\\
\hat{P}_k^{-1}\hat{x}_k&=&\check{P}_k^{-1}\check{x}_k+C_kR_k^{-1}y_k
\end{eqnarray*}
\]
假设定义卡尔曼增益矩阵\(K_k=\hat{P}_kC_kR_k^{-1}\)，对信息矩阵形式进行变换就得到了KF的经典形式：
\[\begin{eqnarray*}
\check{P}_k &=& A_{k-1}\hat{P}_{k-1} A_{k-1}^T+Q_k \\
\check{x}_k &=&A_{k-1}\hat{x}_{k-1}+v_k\\
K_k&=&\check{P}_kC_k^T(C_k\check{P}_kC_k^T+R_k)^{-1}\\
\hat{P}_k &=& (1-K_kC_k)\check{P}_k\\
\hat{x}_k&=&\check{x}_k+K_k(y_k-C_k\check{x}_k)
\end{eqnarray*}
\]
贝叶斯推断推导KF
从贝叶斯推断的角度推导KF比较简洁明了。还是马尔科夫的假设。
假设\(k-1\)时刻系统状态的概率函数已知：
\[p(x_{k-1}|\check{x}_0,v_{1:k-1},y_{0:k-1})=\mathcal{N}(\hat{x}_{k-1},\hat{P}_{k-1})
\]
首先，由线性的运动方程得到预测（Prediction），高斯随机变量通过线性系统：
\[p(x_{k}|\check{x}_0,v_{1:k},y_{0:k-1})=\mathcal{N}(\check{x}_{k},\check{P}_{k})
\]
其中
\[\begin{eqnarray*}
\check{P}_k&=&A_{k-1}\hat{P}_{k-1}A_{k-1}^T+Q_k\\
\check{x}_k&=&A_{k-1}\hat{x}_{k-1}+v_k
\end{eqnarray*}
\]
接下来，由观测方程的到修正（Correction）。为了得到\(p(x_{k}|\check{x}_0,v_{1:k},y_{0:k})\)，我们先求得联合概率密度函数\(p(x_{k},y_k|\check{x}_0,v_{1:k},y_{0:k-1})\)：
\[p(x_{k},y_k|\check{x}_0,v_{1:k},y_{0:k-1})=\mathcal{N}\bigg(
\begin{bmatrix}
\mu_x\\
\mu_y
\end{bmatrix},
\begin{bmatrix}
\Sigma_{xx}& \Sigma_{xy}\\
\Sigma_{yx}&\Sigma_{yy}
\end{bmatrix}\bigg) \mathcal{N}\bigg(
\begin{bmatrix}
\check{x}_k\\
C_k\check{x}_k
\end{bmatrix},
\begin{bmatrix}
\check{P}_k & \check{P}_kC_k^T\\
C_k\check{P}_k& C_k\check{P}_kC_k^T+R_k
\end{bmatrix}\bigg)
\]
然后由2.2.3节从联合概率密度函数求条件概率密度函数的公式可得：
\[p(x_{k}|\check{x}_0,v_{1:k},y_{0:k})=\mathcal{N}(\underbrace{\mu_x+\Sigma_{xy}\Sigma_{yy}^{-1}(y_k-\mu_y)}_{\hat{x}_k},\underbrace{\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx}}_{\hat{P}_k})
\]
代入展开，并定义卡尔曼增益矩阵，有：
\[\begin{eqnarray*}
K_k&=&\check{P}_kC_k(C_k\check{P}_kC_k^T+R_k)^{-1}\\
\hat{P}_k&=&(1-K_kC_k)\check{P}_k\\
\hat{x}_k&=&\check{x}_k+K_k(y_k-C_k\check{x}_k)
\end{eqnarray*}
\]
最优化增益矩阵的角度推导KF
从预测值和观测值都可以得到当前状态的估计，为了得到最终的估计，最直接的思路就是对上面二者进行加权平均。这时候，加权的系数就变得非常重要了，它直接影响最后的结果是否最优。从这个角度出发，我们可以构建一个代价函数，对加权系数求导，并令导数为0，求得增益系数。具体如下：
\[\hat{x}_k=\check{x}_k+K_k(y_k-C_k\check{x}_k)\\
e_k=\hat{x}_k-x_k
\]
定义代价函数：
\[J(K_k)=\frac{1}{2}E[e_k^Te_k]=\frac{1}{2}tr\ E[e_ke_k^T]
\]
而
\[E[e_ke_k^T]=(1-K_kC_k)\check{P}_k(1-K_kC_k)^T+K_kR_kK_k^T
\]
对代价函数关于\(K_k\)求导并令导数为0：
\[\frac{\partial{J(K_k)}}{\partial{K_k}}= -(1-K_kC_k)\check{P}_kC_k^T+K_kR_k=0
\]
整理可得：
\[K_k=\check{P}_kC_k(C_k\check{P}_kC_k^T+R_k)^{-1}
\]
可见，卡尔曼增益矩阵就是最优的加权系数矩阵。
连续时间的状态估计
书中连续时间状态估计采用了高斯过程回归的方法，即轨迹或者说运动方程用高斯随机过程来建模，观测方程仍然用离散时间的线性方程来建模。尽管轨迹的建模方法还有其他方式，比如时间基函数，但书中都没有介绍。书中先介绍了通用的高斯回归方法，然后介绍了一种特殊的运动方程，它使得最终需要求解的线性方程组具有稀疏的结构。这种特殊的运动方程又分为线性时变和线性时不变两种情况介绍。
通用的高斯过程回归：
运动方程和观测方程的建立：
\[x(t)=\mathcal{GP}(\check{x}(t),\check{P}(t,t^{'}))\quad t_0 < t, t^{'}\\
y_k=C_kx(t_k)+n_k\quad t_0<t_1<\cdots <t_K
\]
假设我们要求解\((\tau_0<\tau_1<\cdots<\tau_J)\)时刻的系统状态，记为\(x_{\tau}\)。跟前面离散时间估计中的贝叶斯推断的方法类似，我们先求联合概率密度函数，然后用2.2.3节的公式求得条件概率密度函数，概率密度函数中的均值就是待估计的值，协方差表示了不确定性。很容易的到联合概率密度函数
\[p\bigg(\begin{bmatrix}
x_{\tau}\\
y
\end{bmatrix}\bigg) = \mathcal{N}\bigg(
\begin{bmatrix}
\check{x}_{\tau}\\
C\check{x}
\end{bmatrix} ,
\begin{bmatrix}
\check{P}_{\tau\tau} & \check{P}_{\tau}C^T\\
C\check{P}_{\tau}^T & C\check{P}C^T+R
\end{bmatrix}\bigg)
\]
其中
\[x=\begin{bmatrix}
x(t_0)\\
x(t_1)\\
\vdots\\
x(t_K)
\end{bmatrix},
\check{x}=\begin{bmatrix}
\check{x}(t_0)\\
\check{x}(t_1)\\
\vdots\\
\check{x}(t_K)
\end{bmatrix},
x_{\tau}=\begin{bmatrix}
x(\tau_0)\\
x(\tau_1)\\
\vdots\\
x(\tau_J)
\end{bmatrix},
\check{x}_{\tau}=\begin{bmatrix}
\check{x}(\tau_0)\\
\check{x}(\tau_1)\\
\vdots\\
\check{x}(\tau_J)
\end{bmatrix}\\
y=\begin{bmatrix}
y_0\\
\vdots\\
y_K
\end{bmatrix}, C=diag(C_0,\cdots,C_K),R=diag(R_0,\cdots,R_K)\\
\check{P}=\big[\check{P}(t_i,t_j)\big]_{ij},\check{P}_{\tau}=\big[\check{P}(\tau_i,t_j)\big]_{ij},\check{P}_{\tau\tau}=\big[\check{P}(\tau_i,\tau_j)\big]_{ij}
\]
\(P_{\tau}\)是一个\(J\times K\)的矩阵，不一定是对称方阵，注意公式中它的转置。
利用2.2.3节的公式可以很容易的到条件概率密度函数：
\[p(x_{\tau}|y)=\mathcal{N}(\underbrace{\check{x}_{\tau}+\check{P}_{\tau}C^T(C\check{P}C^T+R)^{-1}(y-C\check{x})}_{\hat{x}_{\tau},mean}, \underbrace{\check{P}_{\tau\tau}-\check{P}_{\tau}C^T(C\check{P}C^T+R)^{-1}C\check{P}_{\tau}^T}_{\hat{P}_{\tau\tau},covariance}) \tag{12}
\]
为了求解\(\tau\)时刻的状态估计，我们先求解观测时刻\(t\)的状态估计，然后用插值的方式的到\(\tau\)时刻的估计。需要注意的是这里的插值并不是对时间插值，而是一种协方差意义上的插值。
具体来说，先假设query时间和观测时间相同，即：\(\tau_k=t_k, J=K\)，那么
\[\check{P}_{\tau\tau}=\check{P}_{\tau}=\check{P}
\]
上面的条件概率密度函数就变为：
\[p(x|y)=\mathcal{N}(\check{x}+\check{P}C^T(C\check{P}C^T+R)^{-1}(y-C\check{x}), \check{P}-\check{P}C^T(C\check{P}C^T+R)C\check{P}^T) \tag{13}
\]
进行恒等变换：
\[p(x|y)=\mathcal{N}(\underbrace{(\check{P}^{-1}+C^TR^{-1}C^T)^{-1}(\check{P}^{-1}\check{x}+C^TR^{-1}y)}_{\hat{x},mean},\underbrace{(\check{P}^{-1}+C^TR^{-1}C)^{-1}}_{\hat{P},covariance})
\]
重新组织一下均值的等式，就有
\[(\check{P}^{-1}+C^TR^{-1}C)\hat{x}=(\check{P}^{-1}\check{x}+C^TR^{-1}y) \tag{14}
\]
又是一个信息矩阵乘以后验估计等于信息向量的形式。推导过程和最后的结论几乎跟离散时间的贝叶斯推断方法一模一样。
公式\((14)\)等价于：
\[\hat{x}=\mathop{\arg \min}_{x}\frac{1}{2}(x-\check{x})^T\check{P}^{-1}(x-\check{x})+\frac{1}{2}(y-Cx)^TR^{-1}(y-Cx) \tag{15}
\]
公式\((15)\)跟离散时间MAP方法的代价函数也是几乎一样的，因此这个与最大后验概率的方法也是等效的。
最后看一下插值。
由\((13)\)重新组织一下均值和协方差的公式可以得到：
\[\check{P}^{-1}(\hat{x}-\check{x})=C^T(C\check{P}C^T+R)^{-1}(y-C\check{x})\\
\check{P}^{-1}(\hat{P}-\check{P})\check{P}^{-T}=-C^T(C\check{P}C^T+R)^{-1}C
\]
代入公式\((12)\)可以得到最终的插值公式
\[\hat{x}_{\tau}=\check{x}+\check{P}_{\tau}\underbrace{C^T(C\check{P}C^T+R)^{-1}(y-C\check{x})}_{\check{P}^{-1}(\hat{x}-\check{x})} = \check{x}+(\check{P}_{\tau}\check{P}^{-1})(\hat{x}-\check{x})\\
\hat{P}_{\tau\tau}=\check{P}_{\tau\tau}-\check{P}_{\tau}\underbrace{C^T(C\check{P}C^T+R)^{-1}C}_{-\check{P}^{-1}(\hat{P}-\check{P})\check{P}^{-T}}\check{P}_{\tau}^T=\check{P}_{\tau\tau}+(\check{P}_{\tau}\check{P}^{-1})(\hat{P}-\check{P})(\check{P}_{\tau}\check{P}^{-1})^T
\]
到这里就可以求出任意时刻的状态估计。
