卡尔曼滤波的推导以及个人理解

一、状态估计概念

由于位姿和路标点都是待估计的变量，我们改变一下记号，令${\mathbf{x}}_k$为$k$时刻的所有未知量。它包含了当前时刻的相机位姿与$m$个路标点。在这种记号的意义下写成：

x k ≜ { x k ,   y 1 ,   .   .   .   ,   y m } (1) {{x}_{k}}\triangleq \left\{ \boldsymbol{{x}_{k}},\text{ }\boldsymbol{{y}_{1}},\text{ }.\text{ }.\text{ }.\text{ },\text{ }{\boldsymbol{y}_{m}} \right\}\tag{1} xk​≜{xk​, y1​, . . . , ym​}(1)

把$k$时刻的所有观测记作${\mathbf{z}}_k$:

$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(2)}$$

现在考虑第$k$时刻的情况。我们希望用过去0到$k$中的数据，来估计现在的状态分布：

$$P\left({\mathbf{x}}_{\mathbf{k}}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\text{\hspace{1em}(3)}$$

下标$0:k$ 表示从 0 时刻到$k$时刻的所有数据。请注意$z_k$来表达所有在$k$时刻的观测数据，注意它可能不止一个，只是这种记法更加方便。 下面我们来看如何对状态进行估计。按照 Bayes 法则，把${\mathbf{z}}_k$与${\mathbf{x}}_k$交换位置，有：

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\text{\hspace{1em}(4)}$$
这里第一项称为似然，第二项称为先验。似然由观测方程给
定，而先验部分，我们要明白当前状态$x_k$是基于过去所有的状态估计得来的。按照$x_{k-1}$为条件概率展开：

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=\int P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\text{\hspace{1em}(5)}$$

如果我们考虑更久之前的状态，也可以继续对此式进行展开，但现在我们假设了马尔可夫性，所以只关心$k$时刻和$k-1$时刻的情况。

$$x_0\oplus u_1\to x_1$$

$$x_1\oplus u_2\to x_2$$

由于假设了马尔可夫性，当前时刻状态只和上一个时刻的状态${\mathbf{x}}_{k-1}$和输入${\mathbf{u}}_k$有关，式（5）中等式右侧第一部分可进一步简化
$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)\text{\hspace{1em}(6)}$$

第二部分可简化为：
$$P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_{k-1}\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{z}}_{1:k-1}\right)\text{\hspace{1em}(7)}$$

这是考虑到$k$时刻的输入量${\mathbf{u}}_k$与$k-1$时刻的状态${\mathbf{x}}_{k-1}$无关，所以我们把${\mathbf{u}}_k$拿掉。

这一项实际是$k-1$时刻的${\mathbf{x}}_{k-1}$状态分布。直觉上看，就是用${\mathbf{x}}_0$、一系列的动作输入${\mathbf{u}}_{1:k}$和一系列观测${\mathbf{z}}_{1:k-1}$估计得到的${\mathbf{x}}_{k-1}$。

这样就可以递推：

$$x_0\oplus u_1\to x_1$$

$$x_1\oplus u_2\to x_2$$

于是，$(6)$和$(7)$的到$(5)$的过程说明了，“如何把$k-1$时刻的状态分布推导至$k$时刻”这样一件事（也就是后面要说的预测部分）。也就是说，在程序运行期间，我们只要维护一个状态量，对它进行不断地迭代和更新即可。进一步，如果假设状态量$\mathbf{x}$服从高斯分布，因为高斯分布可以由一个均值和一个方差唯一确定，那我们只需考虑维护状态量的均值和协方差即可。

如果我们假设系统为线性系统，也就是该系统可以由下面的线性方程来描述：
$$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(8)}$$

${\mathbf{A}}_k$就是状态转移矩阵，有的也会为输入增加控制矩阵$B$

$$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(9)}$$

假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布：

$$w_k\sim N(0,R).v_k\sim N(0,Q)\text{\hspace{1em}(10)}$$

为了简洁我省略了$R$和$Q$的下标。现在，利用马尔可夫性，假设我们知道了$k-1$时刻的后验（在$k-1$时刻看来）状态估计：${\hat{\mathbf{x}}}_{k-1}$ 和它的协方差${\hat{\mathbf{P}}}_{k-1}$，现在要根据 $k-1$ 时刻的输入和观测数据，确定 xk 的后验分布。

为区分推导中的先验和后验，我们在记号上作一点区别：以上帽子${\hat{\mathbf{x}}}_k$表示后验，以下帽子${\check{\mathbf{x}}}_k$表示先验分布：

二、推导

总体的思想：

先验就是预测值，不准的值。后验就是修正过的值，精确值。

有了上一时刻的后验（先别管怎么得到的，反正就是有了）。通过运动方程可以大概地预测出下一时刻的值，这个值不准，因为通过运动的性质进行估计会有误差，所以叫做先验值。同时我们还有观测，可以用观测到的信息修正这个先验值，修正之后得到了更精确的值叫做后验。有了后验，那么下一时刻就可以重复上面的过程，用运动方程预测，用观测方程修正

1、通过运动方程确定先验

通过运动方程确定${\mathbf{x}}_k$的先验分布。根据高斯分布的性质，显然有：

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=N\left({\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}\right)\text{\hspace{1em}(11)}$$

这一步称为预测，它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声），推断当前时刻的状态分布，也就是根据上一时刻的后验${\hat{\mathbf{x}}}_{k-1}$ 和它的协方差${\hat{\mathbf{P}}}_{k-1}$，使用运动方程来预测下一时刻的先验${\check{\mathbf{x}}}_k$ 和它的协方差$\check{\boldsymbol{P}}_{k}

注意这里的$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)$使用了$k$时刻的动作和$k-1$时刻以及之前的观测，并没有利用到$k$时刻的观测，所以是不准的，是个先验的概率。

这个推导是通过线性高斯系统的性质得到的。

记先验的概率分布为：
$${\check{\mathbf{x}}}_k={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R}\text{\hspace{1em}(12)}$$

2、通过观测方程修正先验，得到后验

由观测方程，我们可以计算在某个状态下，应该产生怎样的观测数据：

$$P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)=N\left({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}\right)\text{\hspace{1em}(13)}$$

后验概率也就是式$(13)$和式$(11)$的乘积，也就是由式$(4)$给出的贝叶斯公式。直接相乘非常复杂，但是我们可以利用高斯分布和高斯分布的乘积仍然是一个高斯分布这一性质，先假设结果符合的高斯分布$x_k\sim N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$ 那么：

$$N\left({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k\right)=\eta N\left({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}\right)\cdot N\left({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k\right)\text{\hspace{1em}(14)}$$

把$(15)$左右两侧取$log(\ast )$，系数项变为加上的常数项(与${\mathbf{x}}_k$无关)，指数项相乘变为指数相加，那么指数部分相等：

$${\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)={\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)+{\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)\text{\hspace{1em}(15)}$$

为了求左侧的${\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k$，我们把两边展开，并比较${\hat{\mathbf{x}}}_k$的二次和一次系数。对于二次系数，有：

$${\hat{\mathbf{P}}}_k^{-1}={\mathbf{C}}_k^T{\mathbf{Q}}^{-1}{\mathbf{C}}_k+{\check{\mathbf{P}}}_k^{-1}\text{\hspace{1em}(16)}$$

这里定义一个中间变量$\mathbf{K}$，其实也就是卡尔曼增益(这一步推导需要用的SMW恒等式)：

$$\mathbf{K}={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^T{\mathbf{Q}}^{-1}={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T+\mathbf{Q}\right)}^{-1}\text{\hspace{1em}(17)}$$

在式$(16)$左右两侧乘 ${\hat{\mathbf{P}}}_k$ 有:

$$\mathbf{I}={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^T{\mathbf{Q}}^{-1}{\mathbf{C}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}=\mathbf{K}{\mathbf{C}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}\text{\hspace{1em}(18)}$$

于是有后验协方差${\hat{\mathbf{P}}}_k$：

$${\hat{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{P}}}_k\text{\hspace{1em}(19)}$$

然后再算后验的均值比较一次项的系数，有：

$$-2{\hat{\mathbf{x}}}_k^T{\hat{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k=-2{\mathbf{z}}_k^T{\mathbf{Q}}^{-1}{\mathbf{C}}_k{\mathbf{x}}_k-2{\check{\mathbf{x}}}_k^T{\check{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k\text{\hspace{1em}(20)}$$

利用协方差矩阵对称并且对称矩阵的逆矩阵也对称的性质整理得：
$${\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k={\mathbf{C}}_k^T{\mathbf{Q}}^{-1}{\mathbf{z}}_k+{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\text{\hspace{1em}(21)}$$

两侧乘以${\hat{\mathbf{P}}}_k$并代入式（17），得到了后验均值:
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^T{\mathbf{Q}}^{-1}{\mathbf{z}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\\ & =\mathbf{K}{\mathbf{z}}_k+\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)\end{array}\text{\hspace{1em}(23)}$$

三、总结

总而言之，上面的两个步骤可以归纳为“预测”（Predict）和“更新”（Update）两个步骤：

1、预测

计算先验概率分布（均值和方差）
$${\check{\mathbf{x}}}_k={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}\text{\hspace{1em}(24)}$$

2、更新

(1)计算卡尔曼增益
$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T+\mathbf{Q}\right)}^{-1}\text{\hspace{1em}(25)}$$
(2)计算后验概率分布（均值和方差）
$${\hat{\mathbf{x}}}_k={\mathbf{K}}_k{\mathbf{z}}_k+\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)\text{\hspace{1em}(26)}$$

$${\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k\right){\check{\mathbf{P}}}_k\text{\hspace{1em}(27)}$$

式$(17)可能会让人有些迷惑，我们使用 ${\hat{\mathbf{P}}}_k$表示$\mathbf{K}$，然后又用 $\mathbf{K}$ 来表示 ${\hat{\mathbf{P}}}_k$ ,
有些循环定义的意思。但是注意，其实式$(16)已经可以算出来 ${\hat{\mathbf{P}}}_k$ 了，然后我们用它计算了 $\mathbf{K}$ ，式$(17)$后面的那个等式就是计算出来的不依靠 ${\hat{\mathbf{P}}}_k$ 表示的最终形式

四、通俗解释

$${\hat{\mathbf{x}}}_k={\mathbf{K}}_k{\mathbf{z}}_k+\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k$$

当${\mathbf{C}}_k=1$时也就是观测和状态是同一种数据，比如观测是GPS测量到的车的位置，输入是车的里程计。要估计的量是车的位置。

卡尔曼增益其实就是用来对观测值(GPS测量)和先验值(里程计)做了一个加权平均，只不过这个卡尔曼增益是动态刷新的。
如果更相信观测，那么增大 ${\mathbf{K}}_k$ 如果更相信运动预测，就要减小${\mathbf{K}}_k$

五、实际使用(套公式，参数初始化和整定)

（1）选择状态量，观测量，构建运动方程和观测方程

（2）初始化参数

${\hat{\mathbf{x}}}_k$可以设置为$0$ ，因为不知道初始的${\hat{\mathbf{x}}}_k$，当然直到的话也可以设置为其他的值

${\hat{\mathbf{P}}}_{k-1}$可以设置为$1$，这个值其实关系不大，因为很快就可以收敛。一般取个小值方便收敛，但是注意，一定不能取0

$\mathbf{R}$ 和 $\mathbf{Q}$ 的取值根据实际情况而定。

将式$(24)带入$(25)，不难发现：如果更相信观测，那么增大 ${\mathbf{K}}_k$ ，也就是增大运动预测的噪声(过程噪声)的协方差$\mathbf{R}$降低观测噪声协方差$\mathbf{Q}$，如果更相信运动的预测，那么减小 ${\mathbf{K}}_k$ ，也就是增大$\mathbf{Q}$降低$\mathbf{R}$

这也是符合直觉的，如果相信某个传感器准确，也就是这个传感器数据的方差小。

（3）套公式

$${\check{\mathbf{x}}}_k={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}$$
$$\mathbf{K}={\check{\mathbf{P}}}_k{\mathbf{C}}_k^T{\left({\mathbf{C}}_k{\check{\mathbf{P}}}_k{\mathbf{C}}_k^T+\mathbf{Q}\right)}^{-1}$$
$${\hat{\mathbf{x}}}_k=\mathbf{K}{\mathbf{z}}_k+\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right)$$
$${\hat{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{P}}}_k$$

非线性系统和拓展卡尔曼滤波器EKF

经典卡尔曼滤波器的局限性

经典卡尔曼滤波器只适用于线性系统（式$(12)$），当时常见的系统（比如slam）都是非线性的

$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_k=h\left({\hat{\mathbf{x}}}_k\right)+{\mathbf{v}}_k\end{array}k=1,\dots ,N\text{\hspace{1em}(8)}$$

其中$f()$和$h()$都是非线性函数

通常的做法是，在某个点附近考虑运动方程以及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。令$k-1$时刻的均值与协方差矩阵为 ${\hat{\mathbf{x}}}_{k-1}$, ${\hat{\mathbf{P}}}_{k-1}$。在 $k$ 时刻，我们把运动方程和观测方程，在 ${\hat{\mathbf{x}}}_{k-1}$, ${\hat{\mathbf{P}}}_{k-1}$ 处进行线性化（相当于一阶泰勒展开），有：

$${\mathbf{x}}_k\approx f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)+{\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k$$

记这里的偏导数为：
$$\mathbf{F}={\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}$$

$${\mathbf{z}}_k\approx h\left({\check{\mathbf{x}}}_k\right)+{\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)+{\mathbf{n}}_k$$

记这里的偏导数为：
$$\mathbf{H}={\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}$$

那么，在预测步骤中，根据运动方程有:

$$P\left({\mathbf{x}}_k\mid {\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{0:k-1}\right)=N\left(f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),\mathbf{F}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k\right)$$

这些推导和卡尔曼滤波是十分相似的。为方便表述，记这里先验和协方差的均值为

$${\check{\mathbf{x}}}_k=f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),{\check{\mathbf{P}}}_k=\mathbf{F}{\hat{\mathbf{P}}}_k{\mathbf{F}}^T+{\mathbf{R}}_k$$

然后，考虑在观测中，我们有：

$$P\left({\mathbf{z}}_k\mid {\mathbf{x}}_k\right)=N\left(h\left({\check{\mathbf{x}}}_k\right)+\mathbf{H}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{Q}}_k\right)$$

最后，根据最开始的 Bayes 展开式，可以推导出 xk 的后验概率形式。我们略去中间的推导过程，只介绍其结果。读者可以仿照着卡尔曼滤波器的方式，推导 EKF 的预测与更新方程。简而言之，我们会先定义一个卡尔曼增益:
$${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}^{\mathrm{T}}{\left(\mathbf{H}{\check{\mathbf{P}}}_k{\mathbf{H}}^{\mathrm{T}}+{\mathbf{Q}}_k\right)}^{-1}$$
在卡尔曼增益的基础上，后验概率的形式为：
$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_k-h\left({\check{\mathbf{x}}}_k\right)\right),{\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\check{\mathbf{P}}}_k$$

卡尔曼滤波器给出了在线性化之后，状态变量分布的变化过程。在线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计。而在 SLAM 这种非线性的情况下，它给出了单次线性近似下最大后验估计（MAP）

实际应用1 用陀螺仪数据估计角度

未完待续……敬请关注

实际应用2 融合陀螺仪数据和加速度计估计角度

未完待续……敬请关注