SLAM--状态估计

一、 贝叶斯公式

若存在$\mathbf{X},\mathbf{Y}$随机变量，则$p(x,y)$被称为$(\mathbf{X},\mathbf{Y})$的联合概率密度函数；
边缘概率密度的性质：

性质1：
$$p(x,y)=p(x|y)\cdot p(y)=p(y|x)\cdot p(x)$$

性质2：
$$\int p(x|y)\mathrm{d}x=1,\int p(y|x)\mathrm{d}y=1,$$

性质3：
$$p(x)=\int p(x,y)\mathrm{d}y,p(y)=\int p(x,y)\mathrm{d}x,$$

贝叶斯公式：
$$p(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{p(y|x)p(x)}{\int p(y|x)p(x)\mathrm{d}x}$$
对于分母$p(y)$,我们可以利用边缘密度：
$$p(y)=\int p(x,y)\mathrm{d}y=\int p(y|x)p(x)\mathrm{d}x$$
在贝叶斯推断中，我们把p(x)称为先验，p(x|y)称为后验，p(y|x)称为似然概率；
 
 

二、高斯密度函数

设$\mathbf{X}={\left[\begin{array}{cccc}{\mathbf{X}}_1& {\mathbf{X}}_2& ...& {\mathbf{X}}_n\end{array}\right]}^T$是n维随机向量，$\mathbf{x}={\left(\begin{array}{c}{x}_1,x_2,...,x_n\end{array}\right)}^T$，则$\mathbf{X}$的联合密度函数为：
$$p(x)=\frac{1}{(2\pi {)}^{\frac{n}{2}}(\det \mathbf{B}{)}^{\frac{1}{2}}}\exp (-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }){\mathbf{B}}^{-1}(\mathbf{x}-\mathbf{\mu }))\text{\hspace{1em}(1)}$$
 
记为$\mathbf{X}\backsim \mathbf{N}(\mathbf{\mu },\mathbf{B})$.

其中，$\mathbf{\mu }={\left[\begin{array}{c}\mathbf{E}{X}_1,\mathbf{E}{X}_2,...,\mathbf{E}{X}_n\end{array}\right]}^T$，为各变量的数学期望或均值，$\mathbf{B}=[b_{ij}{]}_{n\times n}$矩阵为协方差矩阵:
$$\mathbf{E}{\mathbf{X}}_k={\mathbf{\mu }}_k,{\mathbf{b}}_{kl}=\mathbf{c}\mathbf{o}\mathbf{v}({\mathbf{X}}_k,{\mathbf{X}}_l)=\mathbf{E}[({\mathbf{X}}_k-\mathbf{E}{\mathbf{X}}_k)({\mathbf{X}}_l-\mathbf{E}{\mathbf{X}}_l{)}^T]\text{\hspace{1em}(2)}$$
高斯分布有一个重要的性质：

若$\mathbf{X}\backsim \mathbf{N}(\mathbf{\mu },\mathbf{B})$，$\mathbf{C}$ 是 $\mathbf{m}\times \mathbf{n}$的矩阵，则m维向量$\mathbf{Y}=\mathbf{C}\mathbf{X}$服从m维的正态分布$\mathbf{N}(\mathbf{C}\mathbf{\mu },\mathbf{C}\mathbf{B}{\mathbf{C}}^{\mathbf{T}})$

 
 

三、联合高斯概率密度函数分解推导

存在多元正态分布$(x,y)$，则联合高斯密度函数为：
$$p(x,y)=\mathbf{N}(\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right])\text{\hspace{1em}(3)}$$
其中${\sum }_{yx}={\sum }_{xy}^T$。

由贝叶斯法则：
$$p(x,y)=p(x|y)\cdot p(y)\text{\hspace{1em}(4)}$$
分解指数p(x,y)的部分：
$$\begin{array}{rl}& (\left[\begin{array}{c}\mathbf{x}\\ \\ \mathbf{y}\end{array}\right]-\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right]{)}^T{\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right]}^{-1}(\left[\begin{array}{c}\mathbf{x}\\ \\ \mathbf{y}\end{array}\right]-\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right])\\ & \\ & =(\mathbf{x}-{\mathbf{\mu }}_x-{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y){)}^T\cdot ({\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx}{)}^{-1}\cdot (\mathbf{x}-{\mathbf{\mu }}_x-{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y))\\ & +(\mathbf{y}-{\mathbf{\mu }}_y{)}^T{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y)\end{array}\text{\hspace{1em}(5)}$$
根据指数分解的结果以及$p(x,y)=p(x|y)\cdot p(y)$可以得到：
$$\begin{array}{rl}p(x|y)& =\mathbf{N}({\mathbf{\mu }}_x+{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y),{\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx})\\ & \\ p(y)& =\mathbf{N}({\mathbf{\mu }}_y,{\sum }_{yy})\end{array}\text{\hspace{1em}(6)}$$
上面的式子是非常重要的，尤其是在推导卡尔曼滤波过程中。

 
 

四、线性高斯系统状态估计

4.1 离散状态的批量估计

 
设状态方程 和 观测方程分别为：

状态方程：${\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{\omega }}_k,{\mathbf{\omega }}_k\backsim \mathbf{N}(0,{\mathbf{Q}}_k)$

观测方程：${\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k,{\mathbf{n}}_k\backsim \mathbf{N}(0,{\mathbf{R}}_k)$

其中${\mathbf{\omega }}_k$和${\mathbf{n}}_k$为噪声，${\mathbf{u}}_k$为输入，${\mathbf{y}}_k$为观测。
 

我们利用 最大后验估计MAP 来进行推导，利用贝叶斯法则，所以MAP的优化目标为：
$$\underset{x}{\mathrm{argmax}}p(x|y,u)=\underset{x}{\mathrm{argmax}}\frac{p(y|x,u)\cdot p(x|u)}{p(y|u)}=\underset{x}{\mathrm{argmax}}p(y|x,u)\cdot p(x|u)\text{\hspace{1em}(7)}$$

1.首先针对其中p(y|x,u)：
$$\ln p(y|x,u)=-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k)+m，m和x不相关$$
最后优化的目标函数为
$$⟹J_y(x)=-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k)\text{\hspace{1em}(8)}$$

2.其次再针对p(x|u):
$$\ln p(x|u)=-\frac{1}{2}(x_k-A_k{x}_{k-1}-u_k{)}^T{Q}_k^{-1}(x_k-A_k{x}_{k-1}-u_k)+n，n和x不相关$$
最后优化的目标函数为
$$⟹J_u(x)=-\frac{1}{2}(x_k-A_k{x}_{k-1}-u_k{)}^T{Q}_k^{-1}(x_k-A_k{x}_{k-1}-u_k)\text{\hspace{1em}(9)}$$

所以目标函数为:
$$\begin{gathered} J(x)=J_u(x)+J_y(x) \\ =-\frac{1}{2}(y_k-C_k{x}_k{)}^T{R}_k^{-1}(y_k-C_k{x}_k)+-\frac{1}{2}(x_k-A_k{x}_{k-1}-u_k{)}^T{Q}_k^{-1}(x_k-A_k{x}_{k-1}-u_k)\text{\hspace{1em}(10)} \end{gathered}$$

我们把$\hat{x}$作为后验估计，$\check{x}$作为先验估计

即：$\hat{x}=\underset{x}{\mathrm{argmax}}J(x)$

我们令$\mathbf{z}=[{\check{\mathbf{x}}}_0,{\mathbf{u}}_1,...,{\mathbf{u}}_{\mathbf{k}}|{\mathbf{y}}_0,...,{\mathbf{y}}_{\mathbf{k}}{]}^T$，$\mathbf{x}=[{\mathbf{x}}_0,...,{\mathbf{x}}_{\mathbf{k}}{]}^{\mathbf{T}}$，并且定义：
$$\mathbf{H}=\left[\begin{array}{ccccc}1& & & & \\ -A_0& 1& & & \\ & -A_1& 1& & \\ & & ...& & \\ & & & -A_{{}_{k-1}}& 1\\ C_0& & & & \\ & C_1& & & \\ & & ...& & \\ & & & C_{k-1}& \\ & & & & C_k\end{array}\right]$$
$$\mathbf{W}=\left[\begin{array}{cc}\mathbf{Q}& \\ & \mathbf{R}\end{array}\right]$$
这时我们的目标函数可以写成：
$$\mathbf{J}(x)=\frac{1}{2}(\mathbf{z}-\mathbf{H}x{)}^T{\mathbf{W}}^{-1}(\mathbf{z}-\mathbf{H}x)\text{\hspace{1em}(11)}$$
由于W矩阵是对称矩阵，对目标函数求导可得：
$$\frac{\partial J(x)}{\partial x}=(\mathbf{H}\hat{x}-\mathbf{z}{)}^T{\mathbf{W}}^{-1}\mathbf{H}\text{\hspace{1em}(12)}$$
令导数等于0：
$$\begin{gathered} (\mathbf{H}\hat{x}-\mathbf{z}{)}^T{\mathbf{W}}^{-1}\mathbf{H}=0 \\ ⟹{\mathbf{H}}^T{\mathbf{W}}^{-1}(\mathbf{H}\hat{x}-\mathbf{z})=0 \end{gathered}$$
所以：
$$({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H})\hat{x}={\mathbf{H}}^T{\mathbf{W}}^{-1}\cdot \mathbf{z}\text{\hspace{1em}(13)}$$

4.2 最大后验估计的协方差

根据贝叶斯法则，可以表示为：
$$p(x|z)=\beta \exp (-\frac{1}{2}(\mathbf{H}x-\mathbf{z}{)}^T{\mathbf{W}}^{-1}(\mathbf{H}x-\mathbf{z})),\beta 为归一化因子\text{\hspace{1em}(14)}$$
联合式13可得：
$$p(x|\hat{x})=\kappa \exp (-\frac{1}{2}(x-\hat{x}{)}^T({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H})(x-\hat{x}))\text{\hspace{1em}(15)}$$
由$x\backsim \mathbf{N}(\hat{x},\hat{P})$可以推断：

其中：$({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}{)}^{-1}=\hat{P}$,即协方差；

4.3 利用稀疏性求解

至此，就可以求出x的最大后验估计，但我们不会直接求$({\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}{)}^{-1}$，一般的方法是采用Cholesky分解或者利用矩阵的稀疏性来解。下面介绍Cholesky分解法：

注意到：
$${\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}=\left[\begin{array}{cccccccc}\ast & \ast & & & & & & \\ \ast & \ast & \ast & & & & & \\ & \ast & \ast & \ast & & & & \\ & & & .& .& .& & \\ & & & & .& .& .& \\ & & & & & \ast & \ast & \ast \\ & & & & & & \ast & \ast \end{array}\right]$$
根据Cholesky分解法将其分解为：
$${\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}=\mathbf{L}{\mathbf{L}}^T\text{\hspace{1em}(16)}$$
其中L为下三角矩阵：

$$\mathbf{L}=\left[\begin{array}{cccccccc}\ast & & & & & & & \\ \ast & \ast & & & & & & \\ & \ast & \ast & & & & & \\ & & .& .& .& & & \\ & & & .& .& .& & \\ & & & & & \ast & \ast & \\ & & & & & & \ast & \ast \end{array}\right]$$
这样我们就可以先求解：
$$\mathbf{L}d={\mathbf{H}}^T{\mathbf{W}}^{-1}\cdot \mathbf{z}\text{\hspace{1em}(17)}$$
之后求解：
$${\mathbf{L}}^T\hat{x}=d$$
由于矩阵的特殊性，我们可以直接将上一步计算的结果直接代入到下一步直接求解，这里不再展开，具体参考：《机器人学中的状态估计》----p48-52

参考博文：

视觉SLAM中的数学——解方程AX=b与矩阵分解：奇异值分解（SVD分解） 特征值分解 QR分解 三角分解 LLT分解
 
 

五、离散卡尔曼滤波

5.1 线性高斯系统

存在状态方程和观测方程：
状态方程：${\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{\omega }}_k,{\mathbf{\omega }}_k\backsim \mathbf{N}(0,{\mathbf{Q}}_k)$

观测方程：${\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k,{\mathbf{n}}_k\backsim \mathbf{N}(0,{\mathbf{R}}_k)$
 
 

5.1.1 利用联合高斯概率密度推导

由贝叶斯公式：
$$p(x_k,y_k|u_k)=p(x_k|y_k,u_k)\cdot p(y_k|u_k)\text{\hspace{1em}(18)}$$
由本文的第三节可以知道，当我们求出联合分布概率密度时，就能求出边缘概率密度；所以我们对$p(x_k,y_k|u_k)$进行求解：
$$p(x_k,y_k|u_k)=\mathbf{N}(\left[\begin{array}{c}{\mathbf{\mu }}_x\\ \\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right])=\mathbf{N}(\left[\begin{array}{c}\check{{\mathbf{x}}_{\mathbf{k}}}\\ \\ {\mathbf{C}}_k\check{{\mathbf{x}}_{\mathbf{k}}}\end{array}\right],\left[\begin{array}{cc}{\sum }_{xx}& {\sum }_{xy}\\ & \\ {\sum }_{yx}& {\sum }_{yy}\end{array}\right])$$
其中
$$\begin{gathered} {\sum }_{xy}=\mathbf{E}[(x_k-\mathbf{E}{x}_k)(y_k-\mathbf{E}{y}_k{)}^T]=E[(x_k-{\check{\mathbf{x}}}_{\mathbf{k}})({\mathbf{C}}_k{\mathbf{x}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_{\mathbf{k}}{)}^T] \\ ⟹{\sum }_{xy}={\sum }_{xx}\cdot {\mathbf{C}}_k^T={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T\text{\hspace{1em}(19)} \end{gathered}$$
由于对称矩阵，所以${\sum }_{yx}={\sum }_{xy}^T={\mathbf{C}}_k\cdot {\check{\mathbf{P}}}_{\mathbf{k}}$

所以：
$$p(x_k,y_k|u_k)=\mathbf{N}(\left[\begin{array}{c}\check{{\mathbf{x}}_{\mathbf{k}}}\\ \\ {\mathbf{C}}_k\check{{\mathbf{x}}_{\mathbf{k}}}\end{array}\right],\left[\begin{array}{cc}{\check{\mathbf{P}}}_{\mathbf{k}}& {\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T\\ & \\ {\mathbf{C}}_k\cdot {\check{\mathbf{P}}}_{\mathbf{k}}& {\mathbf{C}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k\end{array}\right])\text{\hspace{1em}(20)}$$

所以很快求出：
$$\begin{gathered} p(x_k|y_k,u_k)=\mathbf{N}({\mathbf{\mu }}_x+{\sum }_{xy}{\sum }_{yy}^{-1}(\mathbf{y}-{\mathbf{\mu }}_y),{\sum }_{xx}-{\sum }_{xy}{\sum }_{yy}^{-1}{\sum }_{yx}) \\ ⟹=\mathbf{N}({\check{x}}_k+{\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1}(y_k-{\mathbf{C}}_k{\check{x}}_k),(1-{\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T(C_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1}{\mathbf{C}}_k){\check{\mathbf{P}}}_{\mathbf{k}})\text{\hspace{1em}(21)} \end{gathered}$$
 
------------更新------------
为了简化,我们令:
$$\begin{gathered} K_k={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1} \\ {\hat{\mathbf{P}}}_{\mathbf{k}}=(1-K_k{\mathbf{C}}_k)\check{{\mathbf{P}}_{\mathbf{k}}} \\ {\hat{\mathbf{x}}}_{\mathbf{k}}={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_{\mathbf{k}})\text{\hspace{1em}(22)} \end{gathered}$$
$K_k$我们也称为卡尔曼增益

------------预测------------
其中：
$$\{\begin{array}{rl}& {\check{\mathbf{x}}}_{\mathbf{k}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{x}}}_{\mathbf{k}-1}+{\mathbf{u}}_{\mathbf{k}}\\ & \\ & \check{{\mathbf{P}}_{\mathbf{k}}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{P}}}_{\mathbf{k}-1}{\mathbf{A}}_{\mathbf{k}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{k}}\end{array}\text{\hspace{1em}(23)}$$
上面就是卡尔曼滤波的更新过程
 
 

5.1.2 最大后验估计推导

由贝叶斯法则得：
$$\begin{array}{rl}\underset{x_k}{\mathrm{argmax}}p(x_k|y_k,u_k)& =\underset{x_k}{\mathrm{argmax}}\frac{p(y_k|x_k,u_k)\cdot p(x_k|u_k)}{p(y_k|u_k)}\\ & \\ & =\underset{x_k}{\mathrm{argmax}}p(y_k|x_k,u_k)\cdot p(x_k|u_k)\end{array}\text{\hspace{1em}(24)}$$
$p(y_k|u_k)$与要估计的$x_k$无关，所以可以直接忽略。其中：$p(x_k|y_k,u_k)$为后验概率，$p(y_k|x_k,u_k)$为似然，$p(x_k|u_k)$为先验概率。

但直接求后验是非常困难的，所以我们需要来求后者的概率分布：

1.对于先验分布
$$\begin{gathered} p(x_k|u_k)=\mathbf{N}({\check{\mathbf{x}}}_{\mathbf{k}},{\check{\mathbf{P}}}_{\mathbf{k}}) \\ 其中:{\check{\mathbf{x}}}_{\mathbf{k}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{x}}}_{\mathbf{k}-1}+{\mathbf{u}}_{\mathbf{k}},{\check{\mathbf{P}}}_{\mathbf{k}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{P}}}_{\mathbf{k}-1}{\mathbf{A}}_{\mathbf{k}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{k}}\text{\hspace{1em}(25)} \end{gathered}$$
2.似然部分
$$p(y_k|x_k,u_k)=\mathbf{N}({\mathbf{C}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)\text{\hspace{1em}(26)}$$

3.后验估计部分
设有：
$$\begin{gathered} p(x_k|y_k,u_k)=\mathbf{N}({\hat{\mathbf{x}}}_{\mathbf{k}},{\hat{\mathbf{P}}}_{\mathbf{k}}) \\ \therefore \mathbf{N}({\hat{\mathbf{x}}}_{\mathbf{k}},{\hat{\mathbf{P}}}_{\mathbf{k}})=\mu \mathbf{N}({\mathbf{C}}_k{\mathbf{x}}_k,{\mathbf{R}}_k)\cdot \mathbf{N}({\check{\mathbf{x}}}_{\mathbf{k}},{\check{\mathbf{P}}}_{\mathbf{k}})\text{\hspace{1em}(27)} \end{gathered}$$
对于高斯分布来说，我们一般不会在意前面的因子部分，更注重的是高斯密度函数指数展开部分，因为高斯分布的指数部分包含了所有均值和协方差信息。
 
$$({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}}{)}^{\mathbf{T}}{\hat{\mathbf{P}}}_{\mathbf{k}}^{-1}({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}})=({\mathbf{y}}_{\mathbf{k}}-{\mathbf{C}}_{\mathbf{k}}{\mathbf{x}}_{\mathbf{k}}){\mathbf{R}}_{\mathbf{k}}^{-1}({\mathbf{y}}_{\mathbf{k}}-{\mathbf{C}}_{\mathbf{k}}{\mathbf{x}}_{\mathbf{k}})+({\mathbf{x}}_{\mathbf{k}}-{\check{\mathbf{x}}}_{\mathbf{k}}{)}^{\mathbf{T}}{\check{\mathbf{P}}}_{\mathbf{k}}^{-1}({\mathbf{x}}_{\mathbf{k}}-{\check{\mathbf{x}}}_{\mathbf{k}})\text{\hspace{1em}(28)}$$
我们考虑${\mathbf{x}}_k$二次型的系数：
$${\hat{\mathbf{P}}}_{\mathbf{k}}^{-1}={\mathbf{C}}_{\mathbf{k}}^{\mathbf{T}}{\mathbf{R}}_{\mathbf{k}}^{-1}{\mathbf{C}}_{\mathbf{k}}+{\check{\mathbf{P}}}_{\mathbf{k}}^{-1}\text{\hspace{1em}(29)}$$
我们定义一个中间量：$K_k={\hat{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T{\mathbf{R}}_k^{-1}$
$$\therefore {\hat{\mathbf{P}}}_{\mathbf{k}}=(1-K_k{\mathbf{C}}_k)\check{{\mathbf{P}}_{\mathbf{k}}}\text{\hspace{1em}(30)}$$
再分析一次系数：
$$-2{\hat{\mathbf{x}}}_{\mathbf{k}}^{\mathbf{T}}{\hat{\mathbf{P}}}_{\mathbf{k}}^{-1}{\mathbf{x}}_{\mathbf{k}}=-2{\mathbf{y}}_{\mathbf{k}}^{\mathbf{T}}{\mathbf{R}}_{\mathbf{k}}^{-1}{\mathbf{C}}_{\mathbf{k}}{\mathbf{x}}_{\mathbf{k}}-2{\check{\mathbf{x}}}_{\mathbf{k}}^{\mathbf{T}}{\check{\mathbf{P}}}_{\mathbf{k}}^{-1}{\mathbf{x}}_{\mathbf{k}}\text{\hspace{1em}(31)}$$
根据系数并转置，最后表示$\widehat{{\mathbf{x}}_{\mathbf{k}}}$得：
$${\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_{\mathbf{k}})\text{\hspace{1em}(32)}$$
 
 

5.1.3 卡尔曼滤波迭代算法

根据推导，我们的卡尔曼滤波迭代过程为：
------------第一步：预测------------
$$\{\begin{array}{rl}& {\check{\mathbf{x}}}_{\mathbf{k}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{x}}}_{\mathbf{k}-1}+{\mathbf{u}}_{\mathbf{k}}\\ & \\ & \check{{\mathbf{P}}_{\mathbf{k}}}={\mathbf{A}}_{\mathbf{k}}{\hat{\mathbf{P}}}_{\mathbf{k}-1}{\mathbf{A}}_{\mathbf{k}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{k}}\end{array}\text{\hspace{1em}(33)}$$
------------第二步：更新------------
$$\{\begin{array}{rl}{K}_k& ={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{C}}_k^T({\mathbf{C}}_k{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{C}}_k^T+{\mathbf{R}}_k{)}^{-1}\\ & \\ {\hat{\mathbf{P}}}_{\mathbf{k}}& =(1-K_k{\mathbf{C}}_k)\check{{\mathbf{P}}_{\mathbf{k}}}\\ & \\ {\hat{\mathbf{x}}}_{\mathbf{k}}& ={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_{\mathbf{k}})\end{array}\text{\hspace{1em}(34)}$$
卡尔曼滤波是最大后验概率估计，主要根据是高斯分布的线性变换依然是高斯分布，另外高斯密度函数也可以进行相应的线性变换。
 
 

5.2 非线性高斯系统

存在非线性的状态方程和观测方程：

状态方程：${\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{\omega }}_k,{\mathbf{\omega }}_k\backsim \mathbf{N}(0,{\mathbf{Q}}_k)$

观测方程：${\mathbf{y}}_k=h({\mathbf{x}}_k)+{\mathbf{n}}_k,{\mathbf{n}}_k\backsim \mathbf{N}(0,{\mathbf{R}}_k)$

由泰勒展开式：

线性化：
$$f(x_{k-1},u_k)\approx f({\hat{x}}_{k-1},u_k)+\frac{\partial f({\hat{\mathbf{x}}}_{\mathbf{k}-1},{\mathbf{u}}_{\mathbf{k}})}{\partial {\mathbf{x}}_{k-1}}\cdot (x_{k-1}-{\hat{x}}_{k-1})={\check{x}}_k+\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}{|}_{{\hat{\mathbf{x}}}_{\mathbf{k}-1},{\mathbf{u}}_{\mathbf{k}}}\cdot (x_{k-1}-{\hat{x}}_{k-1})$$
$$h(x_k)\approx {\check{y}}_k+\frac{\partial h}{\partial {\mathbf{x}}_k}{|}_{{\check{\mathbf{x}}}_{\mathbf{k}}}(x_k-{\check{x}}_k)$$

我们不再详细推导过程，我们令：
$$\begin{gathered} \mathbf{F}=\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}{|}_{{\hat{\mathbf{x}}}_{\mathbf{k}-1},{\mathbf{u}}_{\mathbf{k}}} \\ \mathbf{H}=\frac{\partial h}{\partial {\mathbf{x}}_k}{|}_{{\check{\mathbf{x}}}_{\mathbf{k}}} \end{gathered}$$

------------第一步：预测------------
$$\{\begin{array}{rl}& {\check{\mathbf{x}}}_{\mathbf{k}}=\mathbf{f}({\hat{\mathbf{x}}}_{\mathbf{k}-1},{\mathbf{u}}_{\mathbf{k}})\\ & \\ & \check{{\mathbf{P}}_{\mathbf{k}}}=\mathbf{F}{\hat{\mathbf{P}}}_{\mathbf{k}-1}{\mathbf{F}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{k}}\end{array}\text{\hspace{1em}(35)}$$
------------第二步：更新------------
$$\{\begin{array}{rl}{K}_k& ={\check{\mathbf{P}}}_{\mathbf{k}}\cdot {\mathbf{H}}^T(\mathbf{H}{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{H}}^T+{\mathbf{R}}_k{)}^{-1}\\ & \\ {\hat{\mathbf{P}}}_{\mathbf{k}}& =(1-K_k\mathbf{H})\check{{\mathbf{P}}_{\mathbf{k}}}\\ & \\ {\hat{\mathbf{x}}}_{\mathbf{k}}& ={\check{\mathbf{x}}}_{\mathbf{k}}+K_k({\mathbf{y}}_k-\mathbf{h}({\check{\mathbf{x}}}_{\mathbf{k}}))\end{array}\text{\hspace{1em}(36)}$$
至此，卡尔曼滤波的算法就推导完成。