Abstract: 因为经常要用到，总是学了忘，记一下，存个地方，方便翻阅。

1.基本概念

以一个正在运动的汽车为例，只向$x$方向做直线运动，在某一时刻 它的信息为： $$ x = [ pos \ v]^T $$ $pos$是汽车此时的坐标，$v$是汽车此时的速度。由于汽车是运动的，我们也想知道它下一时刻的信息： $$ x_{i+1} = [ pos_{i+1} \ v_{i+1}]^T $$ 这种由当前时刻推断下一时刻叫做预测。

而当前时刻的信息$x$ 一定是不准确的，如何通过各种车身传感器推断出车当前的信息叫做滤波。为什么一定不准确呢？因为在实际问题中，我们从传感器上所获取的值是绝对有误差的，有的大有的小，我们无法获取任何的绝对真值 ，但我们希望拿到的值又尽可能地接近真值，这就是卡尔曼滤波的作用。所以可得，我们信息中的$x$向量绝对不是真值。

目前假定汽车在$x$方向上做匀速运动，对于匀速运动，可以获取汽车在下一时刻$x_{i+1}$的信息： $$ pos_{i+1} = pos_{i} + v_{i}t $$ $$ v_{i+1} = v_{i} $$ 写成矩阵形式： $$ \begin{bmatrix} pos_{i+1} \\ v_{i+1} \end{bmatrix} = \begin{bmatrix} 1 & t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} pos_i \\ v_i \end{bmatrix} $$

简写为： $$ x_{i+1} = A*x_i $$ $A$称为 状态转移矩阵 。

2.KF正经数学推理

前置知识贝叶斯估计

以下转述及整理来自《视觉SLAM十四讲从理论到实践第二版》，保持简短有效。

2.1方程的建立

假设车辆正携带某些传感器再未知环境里运动，如何用数学语言描述这件事？首先，这些传感器的数据采集是每隔一段时间进行采样，所以时间并不连续，连续的时间运动变成了离散时刻$t=1,…,K$当中发生的事 。如此，在这些时刻，用$x$来表示车辆所处的位置，各时刻的位置记为$x_1, …, x_K$。

地图与观测方面，假设地图是由多个标志物组成，称为路标，传感器会测量到一部分路标并得到观测数据。不妨设路标有$N$个，用$y_1, …, y_N$表示它们。

什么是运动？ 我们要考虑从$k-1$到$k$时刻，车辆的位置$x$事如何变化的。
什么是观测？ 假设车辆在$k$时刻于$x_k$处探测到了一个路标$y_j$，我们还要考虑如何用数学语言来描述这件事。

运动，用一个通用的、抽象的数学模型来说明： $$ x_k = f(x_{k-1}, u_k, w_k) $$

其中，$u_k$表示传感器的读数或者输入， $w_k$是过程中加入的噪声。用一个一般函数$f$来描述过程。这称之为运动方程 。

噪声的存在使得模型变成了随机模型。通俗说，假设下达“前进1米”的指令，实际上可能前进了0.9米、1.1米，也有可能因为轮胎打滑干脆没有前进。所以如果我们不理会这个噪声，只根据指令来确定位置，可能会与实际相差十万八千里。

还有观测方程 ，描述的是，当车辆在$x_k$位置上看到某个路标点$y_j$时，产生了一个观测数据$z_{k,j}$。用抽象函数$h$来描述这个关系： $$ z_{k,j} = h(y_j, x_k, v_{k, j}) $$

这里的$v_{k, j}$是观测的噪声。

于是SLAM过程可总结为两个基本方程： $$ \begin{cases} x_k = f(x_{k-1}, u_k, w_k), k=1, …, K \\ z_{k,j} = h(y_j, x_k, v_{k, j}), (k,j)\in O \end{cases} $$

其中$O$是一个集合，记录在哪个时刻观察到了哪个路标。

这两个方程描述了最基本的SLAM问题：当知道运动测量的读数$u$，以及传感器的读数$z$时，如何求解定位问题（估计$x$）和建图问题（估计$y$）？这是我们就把SLAM问题建模成了一个状态估计问题 ：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

2.2引入滤波与状态估计（batch/incremental）

由于在SLAM过程中，这些数据时随时间逐渐到来的，所以我们应该有一个方法来获得当前时刻状态的估计，这个估计应该结合过去的数据和此时此刻的新的观测数据。这种方式称为增量/渐进(incremental) 的方法，或者叫滤波器 。另一种方式，则是把数据“攒”起来一并处理，这种方式叫做批量的方法。一般用作后端处理，以寻求全局最优，使用非线性优化算法，这也是SfM(Structure from Motion)的主流做法。

这种极端情况是不实时的，不符合SLAM的运用场景。主要不介绍非线性优化，但是这里对于最大后验估计的说法很重要，只补充这一部分 。

考虑从$1$到$N$的所有时刻，并假设有$M$个路标点。定义所有时刻的机器人位姿和路标点坐标为： $$ x = {x_1, …, x_N}, y = {y_1, …, y_M} $$ 同样的，用不带下标的$u$来表示所有时刻的输入，$z$表示所有时刻的观测数据。那么我们说，从概率学的角度来看，就是已知输入/控制数据$u$和观测数据$z$的条件下，求状态$x,y$的条件概率分布： $$ P(x,y|z,u) $$

特别的，当我们不知道控制输入，只有一张张图像（观测）时，即只考虑观测方程带来的数据时，相当于估计$P(x,y|z)$的条件概率分布，此问题也成为SfM，即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有： $$ P(x,y|z,u) = \frac{P(z,u|x,y)P(x,y)}{P(z,u)} \propto P(z,u|x,y)P(x,y) $$

左侧为后验概率 ，右侧$P(z,u|x,y)$称为似然 (Likehood)，$P(x,y)$称为先验 (Prior)。直接求后验分布是困难的，但是求一个最优状态估计（当x,y为多少时），使得在该状态下后验概率最大化 ，是可行的： $$ {(x,y)^*}_{MAP} = argmax P(x,y|z,u) = argmax P(z,u|x,y)P(x,y) $$

贝叶斯法则的分母部分与待估计的状态$x,y$无关，因而可以忽略。贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积 。当然，我们也可以说，对不起，我不知道机器人位姿或者路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计 (Maximize Likelihood Estimation, MLE)： $$ {(x,y)^*}_{MLE} = argmax P(z,u|x,y) $$

直观地讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据 ”。这就是最大似然估计的直观意义。

2.3线性系统与KF

明天再写，我全懂了！！ 2026-02-02 17:50:28

首先，将路标$y$与位姿$x$进行统一定义，这是因为位姿和路标都是待估计的变量。令$x_k$为k时刻的所有未知量，它包含了当前时刻的相机位姿与$m$个路标点： $$ x_k \overset{\mathrm{def}}{=} \{x_k, y_1, …, y_m\} $$

此时，我们就可以将运动方程和观测方程写得更简洁，注意$x$里已经包含了$y$： $$ \begin{cases} x_k = f(x_{k-1}, u_k) + w_k, \\ z_{k} = h(x_k) + v_k, \end{cases} k = 1, …, N $$

重要的知识点

前面说到了估计，SLAM的过程中变量和方程都是受噪声影响的，所以所取得的值都是有误差的————准确的值是不可能的。为了使之尽可能描述准确，采用概率分布的形式来进行描述，例如：$x$取3的概率为0.2，$x$取5的概率是0.1等等。

此时假定位姿$x$和路标$y$都是服从某种概率分布的随机变量，而不是单单的一个孤零零的数值。一般设定这种概率分布为高斯分布，也就是假定它们服从高斯分布————这意味着只需要存储它们的均值和协方差矩阵即可。均值可以看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。在此处可以理解为在车辆行驶过程中，均值描述它自己的位姿，而协方差描述了这个位姿的不确定性。

那么问题就由如何估计状态量转变为如何估计状态量的高斯分布。

2.3.1抽象的状态推导

接着说明的内容。现在考虑第$k$时刻的情况，我们希望用过去$0$到$k$中的数据来估计现在的状态分布： $$ P(x_k|x_0, u_{1:k}, z_{1:k}) \tag{1} $$ 下标$1:k$表示从$1$时刻到$k$时刻的所有数据。按照贝叶斯法则，把$z_k$与$x_k$交换位置，有： $$ P(x_k|x_0, u_{1:k}, z_{1:k}) \propto P(z_k|x_k) P(x_k|x_0, u_{1:k}, z_{1:k-1}) \tag{2} $$ 第一项为似然，第二项为先验。似然由观测方程给定，而先验部分是基于过去的所有状态估计得来的。至少会受到$x_{k-1}$的影响，以$x_{k-1}$时刻为条件概率展开： $$ P(x_k|x_0, u_{1:k}, z_{1:k-1}) = \int P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) P(x_{k-1}|x_0, u_{1:k}, z_{1:k-1}) dx_{k-1} $$ 对这一步的后续处理产生了一些分歧。大体上讲，一种方法是假设马尔可夫性 ，简单的一阶马氏性认为，$k$时刻状态仅与$k-1$时刻状态有关，与再之前的无关。如果做出这样的假设，就会得到以扩展卡尔曼滤波（EKF） 为代表的滤波器方法。另一种方法是考虑$k$时刻与之前的许多状态都有关系，此时会得到非线性优化 为主体的优化框架。

看滤波器模型。首先当前时刻状态只与上一时刻有关，则右侧第一部分可进一步简化： $$ P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) = P(x_k|x_{k-1}, u_k) $$ 这里把$k-1$之前的无关的时刻全部去除，只保留$k-1$到$k$的转换形式。第二部分可简化为： $$ P(x_{k-1}|x_0, u_{1:k}, z_{1:k-1}) = P(x_{k-1}|x_0, u_{1:k-1}, z_{1:k-1}) $$ 此式只把$u_k$拿掉。实际意义是$k-1$时刻是由过去的时刻推导而来。我们对比式(1)，我写在下面： $$ P(x_k|x_0, u_{1:k}, z_{1:k}) $$ 具有一样的形式。所以这一系列方程是在说明，我们实际在做的是 “如何把$k-1$时刻的状态分布推导至$k$时刻” 这样一件事。⭐⭐⭐

2.3.2具体的线性高斯KF

由线性高斯系统，可得具体的运动和观测方程： $$ \begin{cases} x_k = A_k x_{k-1} + u_k + w_k, \\ z_{k} = C_k x_k + v_k, \end{cases} k = 1, …, N $$ 并假设所有的状态和噪声都满足高斯分布，高斯分布的相关知识可参考《视觉SLAM十四讲从理论到实践第二版》的附录部分，不贴了 。即： $$ w_k \sim N(0, R) \quad v_k \sim N(0, Q) $$

特殊记号

为了区分推导中的先验和后验，做了记号上的区别：

先验（下帽子）:$\check{x_k}$

后验（上帽子）:$\hat{x_k}$

后验是当前时刻的相对准确的值，得到当前时刻的先验后需要 计算后验 才是最终结果。

目前已知$k-1$时刻的所有数据，数学化描述即，已知$k-1$时刻的后验高斯分布 ————包括后验状态估计$\hat{x_{k-1}}$及其协方差矩阵$\hat{P_{k-1}}$。⭐⭐⭐⭐⭐此时的目标：根据$k$时刻的输入和观测数据，确定$x_k$的后验分布。

step 1：通过运动方程确定$x_k$的先验分布

这一步是线性的，而高斯分布的线性变换仍然是高斯分布。所以显然有（2.3.1的第二个式子的先验部分）： $$ P(x_k|x_0, u_{1:k}, z_{1:k-1}) = N(A_k \hat{x_{k-1}} + u_k, A_k \hat{P_{k-1}} {A_k}^T + R) $$ 这一步是通过刚刚的运动方程得来的，称为预测（predict） 。它显示了如何让从上一个时刻的状态，根据输入信息（但是有噪声）推断当前时刻的状态分布。这个分布也就是先验，把上面括号里的拿下来。记：⭐⭐⭐ $$ \check{x_k} = A_k \hat{x_{k-1}} + u_k, \quad \check{P_k} = A_k \hat{P_{k-1}} {A_k}^T + R $$ 这非常自然。一方面，显然这一步状态的不确定度要变法，因为噪声增加。

step 2：通过观测方程确定$x_k$的似然

另一方面，由前面写的观测方程，我们可以计算在某个状态下应该产生怎样的观测数据： $$ P(z_k|x_k) = N(C_k x_k, Q) $$

step 3：通过乘积确定$x_k$的后验（更新(update) 的功能）

这个时候，可以发现在2.3.1第二个式子里，右侧的似然和预测全都有了。现在要计算它们的乘积以获得关于$x_k$的后验高斯分布。先设结果为$x_k \sim N(\hat{x_k}, \hat{P_k})$，那么： $$ N(\hat{x_k}, \hat{P_k}) = N(C_k x_k, Q) · N(\check{x_k}, \check{P_k}) $$

插播高斯分布的高维形式

$$ p(x) = \frac{1}{\sqrt{{(2 \pi)}^N det(\Sigma)} } exp \left( -\frac{1}{2} {(x-\mu)}^T \Sigma^{-1} {(x-\mu)}\right) $$

稍微讨巧的办法。既然已经知道等式两侧都是高斯分布，那就只需比较指数部分，无须理会高斯分布前面的因子部分。指数部分很像一个二次型的配方，推导一下。首先把指数部分展开，有（这里的等号并不严格）： $$ {(x_k-\hat{x_k})}^T {\hat{P_k}}^{-1} (x_k - \hat{x_k}) = {(z_k - C_k x_k)}^T Q^{-1} (z_k - C_k x_k) + {(x_k-\check{x_k})}^T {\check{P_k}}^{-1} (x_k - \check{x_k}) $$ 求解方式：比较两侧$x_k$的二次和一次系数。对于二次系数： $$ {\hat{P_k}}^{-1} = C_k^T Q^{-1} C_k + {\check{P_k}}^{-1} $$ 于是我们先得到了后验协方差 的计算过程。定义一个中间变量，这是为了后面避免写长串的式子： $$ K = \hat{P_k} C_k^T Q^{-1} \tag{K} $$ 在前面那个式子${\hat{P_k}}^{-1}$上左右，进行左乘$\hat{P_k}$，有： $$ I = \hat{P_k} C_k^T Q^{-1} C_k + \hat{P_k} {\check{P_k}}^{-1} = K C_k + \hat{P_k} {\check{P_k}}^{-1} $$ 可得后验协方差 ：⭐⭐⭐ $$ \hat{P_k} = (I - K C_k) \check{P_k} $$

比较一次项的系数，有： $$ -2 {\hat{x_k}}^T {\hat{P_k}}^{-1} x_k = -2 z_k^T Q^{-1} C_k x_k - 2 {\check{x_k}}^T {\check{P_k}}^{-1} x_k $$ 整理（取系数并转置）得： $$ {\hat{P_k}}^{-1} x_k = C_k^T Q^{-1} z_k + {\check{P_k}}^{-1} \check{x_k} $$ 两侧乘以$\hat{P_k}$并带入上面的(K)式，得到后验均值 表达式：⭐⭐⭐ $$ \begin{align*} \hat{x_k} &= \hat{P_k} C_k^T Q^{-1} z_k + \hat{P_k} {\check{P_k}}^{-1} \check{x_k} \\ &= K z_k + (I - K C_k) \check{x_k} \\ &= \check{x_k} + K(z_k - C_k \check{x_k}) \end{align*} $$

2.4线性高斯KF公式总结

预测： $$ \check{x_k} = A_k \hat{x_{k-1}} + u_k, \quad \check{P_k} = A_k \hat{P_{k-1}} {A_k}^T + R $$
更新：先计算$K$，它又称为卡尔曼增益 $$ K = \check{P_k} C_k^T {(C_k \check{P_k} C_k^T + Q_k)}^{-1} $$ 然后计算后验概率分布： $$ \begin{align*} \hat{x_k} &= \check{x_k} + K(z_k - C_k \check{x_k}) \\ \hat{P_k} &= (I - K C_k) \check{P_k} \end{align*} $$

3.扩展卡尔曼滤波EKF

SLAM过程中的函数和方程通常是非线性的，一个高斯分布经过非线性变换后，往往不再是高斯分布。在这个的处理中简单来说是将非线性的表达近似成一个线性的函数，然后利用上述的线性高斯进行求解，这叫做扩展卡尔曼滤波。

通常来说对SLAM方程进行一阶泰勒展开，值保留一阶项，即线性的部分。回顾一下SLAM方程： $$ \begin{cases} x_k = f(x_{k-1}, u_k) + w_k, \\ z_{k} = h(x_k) + v_k, \end{cases} k = 1, …, N $$ 对$f$和$h$进行一阶泰勒展开： $$ \begin{cases} x_k \approx f(\hat{x_{k-1}}, u_k) + \left.\frac{\partial f}{\partial x_{k-1}}\right\rvert_{\hat{x_{k-1}}} (x_{k-1} - \hat{x_{k-1}}) + w_k, \\ z_{k} \approx h(\check{x_k}) + \left.\frac{\partial h}{\partial x_{k}}\right\rvert_{\check{x_{k}}} (x_{k} - \check{x_{k}}) + v_k, \end{cases} $$ 设： $$ F = \left.\frac{\partial f}{\partial x_{k-1}}\right\rvert_{\hat{x_{k-1}}} $$ $$ H = \left.\frac{\partial h}{\partial x_{k}}\right\rvert_{\check{x_{k}}} $$

则预测的式子可以写成： $$ P(x_k|x_0, u_{1:k}, z_{1:k-1}) = N(f(\hat{x_{k-1}}, u_k), F \hat{P_{k-1}} {F}^T + R_k) $$ 所以先验可以写作： $$ \check{x_k} = f(\hat{x_{k-1}}, u_k), \quad \check{P_k} = F \hat{P_{k-1}} {F}^T + R_k $$

观测的式子为： $$ P(z_k|x_k) = N(h(\check{x_k}) + H (x_k - \check{x_k}), Q_k) $$

不妨设卡尔曼增益$K_k$ : $$ K_k = \check{P_k} H^T {(H \check{P_k} H^T + Q_k)}^{-1} $$

所以后验概率可写为：⭐⭐⭐⭐⭐ $$ \begin{align*} \hat{x_k} &= \check{x_k} + K_k(z_k - h (\check{x_k})) \\ \hat{P_k} &= (I - K_k H) \check{P_k} \end{align*} $$

Last modified on 2026-01-29