高翔【自动驾驶与机器人中的SLAM技术】学习笔记

高翔【自动驾驶与机器人中的SLAM技术】学习笔记（二）——带着问题的学习；一刷感受；环境搭建

带着问题

为了更好的学习，我们最好带着问题去探索。

第一：核心问题与基础知识

如上图：这本书介绍了SLAM相关的核心问题和基础知识。王谷博士给我们做了梳理：

• ESKF
• 预积分
• 卡尔曼滤波器
• 因子图优化
• 松耦合/紧耦合
• 激光SLAM
• 高精定位
• …
• 点云处理

第二：复现与实践

这本书的另一个意义：我们直接读SLAM相关的论文，可能知道大概意思，但是工程实践很难。复现难，但是这本书提供了一个从论文到实现的指引。哪怕我们没有原论文源码，也可以基于其原理，写出自己的SLAM实现。这对大部分人是非常有价值和帮助的。

第三：带着问题去探索：

• 如何利用IMU产品手册设置IMU运动方程噪声参数
• NDT有什么高效直接的实现方法
• 松耦合/紧耦合的LIO

第四：不只自动驾驶，还可VR、AR

做VR或者AR也要用到SLAM。同样要用：卡尔曼滤波、预积分、子地图、回环检测等技术。

---

---

作者自己在github中阐述：

• 本书大概是您能找到的类似材料中，数学推导和代码实现最为简单的书籍。
• 在这本书里，您会复现许多激光SLAM中的经典算法和数据结构。
  • 您需要自己推导、实现一个误差状态卡尔曼滤波器(ESKF)，把IMU和GNSS的数据喂给它，看它如何推算自己的状态。
  • 您还会用预积分系统实现一样的功能，然后对比它们的运行方式。
  • 接下来您会实现一遍2D激光SLAM中的常见算法：扫描匹配、似然场、子地图，占据栅格，再用回环检测来构建一个更大的地图。这些都需要您自己来完成。
  • 在激光SLAM中，您也会自己实现一遍Kd树，处理近似最近邻，然后用这个Kd树来实现ICP，点面ICP，讨论它们有什么可以改进的地方。
  • 然后您会实现经典的NDT算法，测试它的配准性能，然后用它来搭建一个激光里程计。它比大部分现有LO快得多。
  • 您也会实现一个点面ICP的激光里程计，它也非常快。它工作的方式类似于Loam，但更简单。
  • 您会想要把IMU系统也放到激光里程计中。我们会实现松耦合和紧耦合的LIO系统。同样地，您需要推导一遍迭代卡尔曼滤波器和预积分图优化。
  • 您需要把上面的系统改成离线运行的，让回环检测运行地充分一些。最后将它做成一个离线的建图系统。
  • 最后，您可以对上述地图进行切分，然后用来做实时定位。
• 本书的大部分实现都要比类似的算法库简单的多。您可以快速地理解它们的工作方式，不需要面对复杂的接口。
• 本书会使用非常方便的并发编程。您会发现，本书的实现往往比现有算法更高效。当然这有一部分是历史原因造成的。

一刷的感受：

一：不会，不懂

我说一下读第一遍的感受：好多东西根本不懂。

这时候的做法：分为两个流派：

• 一个流派：按部就班，就按照这个流程，循序渐进。一点点走上去。
• 一个流派：直捣黄龙，逢山开路，遇水搭桥。

1、先说第一个流派的：

• 好处是一步一个脚印，而且每一步都有额外的收获，每一步的收获，都会在以后无限有用。因为技术是一种层层堆叠的东西。你如果就是做技术的，那么静下心来踏踏实实的走上去，非常好。因为代表了无限的可能。因为你基础牢固，内功扎实。你修的是内功。以后想创新想突破都容易。碰到问题，知道如何完善，甚至优化。
• 问题是慢，而且容易走偏。这里说最优化和矩阵论，是研究生课程，你如果是研究生，修过这个心法，正好，不是事。如果你是本科生，如何自处。重新学习一遍这两门课最少也得两三周。你是学生还好，你如果是工作的人。如何处理。
• 这两门课其实是非常基础和实用的课程，如果走第一个流派，真心应该研究一下。

2、另一个流派：

• 碰到不会的再学。他说最优化和矩阵论，那么当碰到读不懂的地方时，直接借助方便的搜索引擎去学习，明确你的学习目标，他说最优化的什么东西。知道

  专业名词

  ，然后弄明白这个

  专业名词

  做什么，找

  一文读懂

  专业名词，借助B站这类网站去找课程学习。

  • （我记得有人分享过：如何快速学习一个行业：找20个这个行业最高频的专业词汇，无论是与人沟通还是搜索，还是看视频课，弄懂他们之后，这个行业你就入门了）

• 逢山开路，遇水搭桥，碰到不会的再去学，这样有目的的去学习，把有限的精力，集中在你的目标上。

• 不足就是创新不足，后劲不足。但也要看你具体要做啥。你的目标大小。目标就是搞懂，就别整其他没用的。

我一刷，只看我能看懂的。

学习源码。看看这本书，到底都是什么内容，哪些工作中可以直接拿来用。哪些是你不会，但是后面你也不用的。哪些是你不会，后面搞不定的，这些是二刷必须要弄会的。这个是广度。

我二刷，要把看不懂的，弄懂。

这种提高，是深度的。

二、ROS

ROS是做SLAM必须要会的，这本书不要求，但是你真心要会。我是跟赵虚左和古月居学的。

三、PCL

他虽然没提，但是你也要会。可以不专精，但是翻译官网的那本书过一遍是有意义的。

四、C++

你可以在本书学习中跟着高翔的源码一起学。特别是一些C++的新特性。多读读他的源码很有价值。高翔不仅书有营养，代码也写的相当好。

---

---

环境搭建

• 操作系统：Ubuntu20.04
• C++标准：C++17
• 硬盘空间：如果是虚拟机，请考虑笔记本单独有个400G左右的空间。因为rosbag的包都比较大。数据集有270GB。而且虚拟机也越来越大，注意记得清理
• 清理虚拟机办法：

• 本书源码：https://github.com/gaoxiang12/slam_in_autonomous_driving
• 本书数据集：百度网盘 请输入提取码百度网盘为您提供文件的网络备份、同步和分享服务。空间大、速度快、安全稳固，支持教育网加速，支持手机端。注册使用百度网盘即可享受免费存储空间https://pan.baidu.com/share/init?surl=ELOcF1UTKdfiKBAaXnE8sQ&pwd=feky

---

高翔【自动驾驶与机器人中的SLAM技术】学习笔记（三）基变换与坐标变换；微分方程；李群和李代数；雅可比矩阵

一、基变换与坐标变换

字小，事不小。

因为第一反应：坐标咋变，坐标轴就咋变呀。事实却与我们想象的相反。这俩互为逆矩阵。

第一次读没有读明白，后面到事上才明白。

起因是多传感器标定：多传感器，就代表了多个坐标系，多个基底。激光雷达和imu标定。这个标定程序，网上，我搜了一下。

· 浙大的LI-Calib，

· 港大的LiDAR_IMU_Init

跑测的结果，始终是相反的。跟上面的情况对上了。所以我们必须要弄懂这个互为逆矩阵咋回事。

1、基变换公式：

旧基底：$x_1,x_2,...,x_n$

新基底：$y_1,y_2,...,y_n$

新基底下向量用旧基底线性表示：

$\{\begin{array}{cccc}{y}_1=c_{11}{x}_1+c_{21}{x}_2+...+c_{n1}{x}_n& & & \\ y_2=c_{12}{x}_1+c_{22}{x}_2+...+c_{n2}{x}_n& & & \\ ......& & & \\ y_n=c_{1n}{x}_1+c_{2n}{x}_2+...+c_{nn}{x}_n& & & \end{array}$
======>>>>>>>>>简化

$\left(y_1,y_2,...,y_n\right)=\left(x_1,x_2,...,x_n\right)C$

Y = XC

$C=\left[\begin{array}{cccc}{c}_{11}& c_{12}& ...& c_{1n}\\ c_{21}& c_{22}& ...& c_{2n}\\ ...& ...& ...& ...\\ c_{n1}& c_{n2}& ...& c_{nn}\end{array}\right]$

C为旧基底改变为新基底的过度（变换）矩阵。

2、坐标变换公式：

向量$\underset{P}{\to }$在新旧两组基下的坐标表示：

旧基坐标表示：$\alpha ={\left({\xi }_1,{\xi }_2,...,{\xi }_n\right)}^T$

新基坐标表示：$\beta ={\left({\eta }_1,{\eta }_2,....,{\eta }_n\right)}^T$

$$\begin{gathered} \underset{P}{\to }=(x_1,x_2,...,x_n)\alpha =(y_1,y_2,...,y_n)\beta =(x_1,x_2,...,x_n)C\beta \\ \Rightarrow \alpha =C\beta \\ \Rightarrow \beta =C^{-1}\alpha \end{gathered}$$

结论：互为逆矩阵，也就解释了，为何标定时，求出来的是个相反的值了。

---

---

---

---

二、 我那有限的微积分知识呀。

1、李群视角下运动学

我向读到此处的朋友请教，请给我一个指引，我需要看一下哪方面的知识。

我用有限的回忆，理解如下：如有不对，请大家给我指引一下。

$f^{\prime }(x)=f(x)\cdot w$

一个函数的导数，是它自身乘以一个常数，第一反应：这个函数应该是$e^x$形式。然后有个系数w，这个函数呼之欲出了。

$f(x)=C{e}^{wx}$

因为只考虑瞬时变化，所以：在$t_0$时刻附近的形式为：

$f(x)=f(t_0)e^{(w(x-t_0))}=f(t_0)exp(w(x-t_0))$

换成随时间变换的R(t)形式，并带入泊松方程。

$R(t)=R(t_0)exp(w^{\wedge }(t-t_0))=R(t_0)e^{w^{\wedge }(t-t_0)}=R(t_0)e^{w^{\wedge }\Delta t}$

然后联系指数映射形式：将时间差用$\Delta t$代替，新的李群R(t)是在R(t0)上瞬时扰动产生的，所以有了2.55的公式。

下一步泰勒展开：一阶展开。后面公式通顺了。

关注一下：泊松公式：记忆一下，对李群视角下的运动学公式：

李群（旋转矩阵这个李群）求导之后，是一个自身，乘以一个公式2.52的反对称矩阵，反对称矩阵都可以写成一个向量形式。

所以结论：旋转矩阵这个李群求导之后，就是原矩阵乘以一个向量（这个向量是反对称矩阵转的）

PS：公式中微分结果为$\dot{R}=R{\omega }^{\wedge }$中，物理意义上称$\omega$为瞬时角速度。

后面读读第五部分：雅可比矩阵详解。感受更深。

二刷感受：第2.2节运动学。不同形式下，如何描述旋转更新量。

微分方程描述的就是这个旋转更新量。有这个更新量，就可以近似求解逼近时刻微小扰动之后的旋转姿态。2.54，2.55，2.56都是在描述这个逼近时刻的姿态（旋转部分）。 只是不同的近似形式。

方法论：这一节结合2.3节，我反复读了好几遍。每次读完， 都间隔几天，去读读其他人的理解。

我隐约记得：读过一本书中写过的学习方法：

渡边康弘《高效阅读》或者《如何高效学习：1年完成MIT4年33门课程的整体性学习法》中介绍的方法：如果你想快速成为一个领域内的专家，就把这方面的书全部拿来，全部通读一遍。因为书中大量重复内容，从不同视角反复解释。你总能明白。

不要觉得很慢，因为重复内容，所以很快就可以读完。

吃饼理论：

· 不要反复吃同一张饼，不要一张饼反复吃，要看看别人怎么吃，别人是蘸酱还是卷菜。

· 不要盯住游戏使劲玩，要看看别人怎么玩，学学别人的攻略。

· 听听别人对这个问题的不同看法和视角。

2、四元数视角下运动学

下一页的四元数求导：

四元数求导之后：就是一个原四元数乘以一个纯虚四元数。微分方程形式还是类似指数形式。

纯虚四元数有自己的运算特征。所以可以进一步简化。详情见书中内容。

只说结论：一个纯虚四元数的指数映射结果为单位四元数。

我们都是提李代数的指数映射为李群，这边是纯四元数的指数映射，所以李代数和四元数一定有一个对应关系。

3、四元数与李代数关系

结论：

· 四元数表达的角速度正好是SO(3)李代数的一半。

· 四元数在旋转一个矢量时要乘两遍。

这个地方的结论与我们之前在25页中乘两次对应上了。

线索再次串联起来。所以高博的书很有深度。很有结构。

乘一次，旋转一半，乘另一次，旋转另一半。两次乘，完成转个旋转。

这个四元数一半的问题，我们暂且记下。后面再来一遍。

---

---

三、李群和李代数

李代数加小的旋转，李群乘小的扰动，BCH线性近似。这里注意BCH中引入了一个雅可比矩阵。

---

---

---

四、雅可比矩阵

在讲BCH近似时：对于指数乘法的运算发现，两个同底指数相乘，并非我们理解的如下公式：

$$\begin{gathered} e^{x+y}=e^x\cdot e^y \\ {e}^x\cdot e^y=e^{x+y} \end{gathered}$$

而是多了个雅可比矩阵。这是因为对于加法的定义不同。

当我们对旋转的函数求导时，有两种定义：

· 一种定义在向量层面，复杂

· 一种定义在扰动层面，简洁

求导时，可以基于四元数q或者旋转矩阵R都可以求导。

结论：对于四元数和旋转矩阵求导，雅可比矩阵都可以使用同一个。

基于四元数q时，需要记住，需要右乘$\frac{1}{2}{\left[1,\omega \right]}^T$。

代码演示如何更新姿态（旋转角度）：圆周运动，单位时间内角速度是个定值。所以旋转的角度也是个定值。用角速度$\omega$乘以时间差$dt$即可得到角度。比如代码中的else中的代码。更新四元数却需要右乘$\frac{1}{2}{\left[1,\omega \right]}^T$。（四元数描述的变换角度是实际变换角度的一半。即一个四元数实际描述一半的角度。因此由角度构造的四元数是所求四元数的2倍。所以必须要乘以1/2）。用这个一半的角度的四元数，再构造实际旋转矩阵SO(3)时，才可以得到正确的值。

$\omega$是角速度矢量，$\omega$乘以$dt$就是一个旋转矢量$\varphi$

这个旋转矢量,通过else中的指数映射变成角度变换的旋转矩阵的李群SO3形式。

Vec3d omega(0, 0, angular_velocity_rad);  // 角速度矢量
 
 
while (ui.ShouldQuit() == false)
{
    // 更新自身位置
    Vec3d v_world = pose.so3() * v_body;
    pose.translation() += v_world * dt;
 
    // 更新自身旋转
    if (FLAGS_use_quaternion) {
        Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
        q.normalize();
        pose.so3() = SO3(q);
    } else {
        pose.so3() = pose.so3() * SO3::exp(omega * dt);

Quatd q = pose.unit_quaternion() * 
Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);

· 首先获取物体当前的旋转四元数，pose.unit_quaternion()，

· 然后根据***角速度更新四元数，***通过四元数相乘的方式。角速度乘以一个更新时间：更新量为角速度乘以时间除以2。

· normalize()归一化：由于浮点数的计算误差，四元数可能会逐渐偏离单位长度，归一化可以确保四元数保持单位长度，这是四元数表示旋转的一个重要性质。

· pose.so3() = SO3(q); 这行代码将更新后的四元数转换为 SO3 类型，并设置为物体的新旋转。SO3 是一个类，它可能是用于表示三维空间中的旋转的类。

· 在代码中要知道四元数Quatd q描述的就是四元数形式下的旋转矩阵。这个四元数形式要变成旋转矩阵通过SO3(q)来实现变换。

· 四元数更新时，乘以上式中的$\frac{1}{2}{\left[1,\omega \right]}^T$。然后再乘以角速度omega乘以dt（时间刻度）。

·

· 旋转矩阵直接乘反而很简单。

·

· 这个w在代码中就是角速度矢量omega。

---

---

---

五、雅可比矩阵详解

参考的链接雅可比矩阵几何意义的直观解释及应用-CSDN博客我们知道我们要研究的关键专业词汇叫：多元向量值函数。

咱们先写自己懂得。从专业词汇切入。

【高等数学笔记】多元向量值函数的导数与微分（简单回忆一下性质即可）

1、多元向量值函数

概念定义：输入是一个n维向量，函数输出是一个m维向量。可以看成m个n元向量函数。

雅可比行列式是坐标变换理论的基础之一，在数学分析***隐函数(无法知道或者无法描述的)***理论中发挥着重要作用。

设有n元向量值函数

$\mathbf{f}:U(x_0)\subseteq {\mathbb{R}}^n\to {\mathbb{R}}^m$

该函数可以表示成：$\mathbf{x}$是n维的，$\mathbf{y}$是m维的。

$\mathbf{f}(\mathbf{x})=\left[\begin{array}{c}{f}_3(\mathbf{x})\\ f_3(\mathbf{x})\\ ...\\ f_m(\mathbf{x})\end{array}\right]=\left[\begin{array}{c}{f}_1(x_1,x_2,x_3,...,x_n)\\ f_2(x_1,x_2,x_3,...,x_n)\\ ...\\ f_m(x_1,x_2,x_3,...,x_n)\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ ...\\ y_m\end{array}\right]$

它将n维空间中的点$\mathbf{x}$映射为m维空间中的点$\mathbf{f}(\mathbf{x})$。

我找到一张神经网络n维输入、m维输出的网络图。便于横向对比理解。

· 从n维输入如何得到m维输出的。新来一个数据如何得到准确的预测。（神经网络干的活。或者说我们要求解的隐函数模型要干的活）

· 就是预测的活。人类对于预测未来（未知）这件事耿耿于怀。

· 不理解不影响大局。

若$\mathbf{f}$的每个分量$f_1,f_2,...,f_n$都在点$x_0$处可微，则我们定义$\mathbf{f}$在$x_0$处的***一阶导数（雅可比矩阵）***为：

简洁表现的雅可比矩阵：

2、雅可比矩阵的用途

仿射变换：

所谓仿射变换（或者叫仿射映射），就是两个变换的复合：线性及平移。具体来说，仿射变换就是指将一个向量空间进行一次线性变换再加上一次平移，变换到另一个向量空间。

局部仿射逼近：

描述局部逼近。

雅可比矩阵用来表示向量值函数的局部仿射逼近。由多元函数的泰勒展开可得，

$f(x_0+\Delta x)=f(x_0)+J_f\Delta x+O({\Vert \Delta x\Vert }^2)$

我们把展开式中的高阶项省略掉，得到向量值函数的仿射逼近，

$f(x_0+\Delta x)\approx f(x_0)+J_f\Delta x$

右侧的两项正是一个仿射映射。

一般的函数$\mathbf{f}$在$x_0$处的附近区域不一定是线性的，不能用一个矩阵去精确表示它。但是，当这个局部非常小的时候，我们可以用一个仿射映射去近似逼近它。

就是站在点$x_0$处，各个方向走一小步$\Delta x$，函数值$f(x_0+\Delta x)$与函数值$f(x_0)$的差（更新量）可以用步子$\Delta x$的一个线性变换来近似表示，即有

$\Delta f=A\Delta x$

也就是说，用一个矩阵就可以去逼近表示函数的微小局部了。当然，每一点处的矩阵可能是不同的。那么这个矩阵具体是怎么样的呢？对，你已经看到过了，就是本文的主角，雅可比矩阵。

我们把雅可比矩阵，

代入下式（此处就直接用等号吧），

$\Delta f=f(x_0+\Delta x)-f(x_0)=J_f\Delta x$

得：

这样相当于：把更新量求解出来了。在SLAM中每次扰动的值，做了近似估计。

多元向量值函数$f$在每个可微分的点处的导数或微分可以用一个矩阵表示，即所谓的雅可比矩阵。

导数怎么是一个矩阵呢？因为这是多元向量值函数的导数，跟一元数量函数的导数相比，在形式上自然应该有所升级。从形式上看有如下的关系，

详细地说，如果$f(x)$在x处是可微的，$h$是一个位移向量，表示定义域内的一小步，则(矩阵)向量乘积$J_{f}h$也是一个位移向量，用于近似表示$h$这一小步引起的函数在值域中的位移向量$f(x+h)-f(x)$

即：

$f(x+h)-f(x)=J_f(x)h$

这个映射将x附近的点y作如下映射：

$f(y)=f(x)+J_f(x)(y-x)$

它是对点x所在的微小区域的函数f(y)的最佳仿射近似。

该仿射映射中的线性部分的系数矩阵$J_f(x)$称为$f$在$x$处的导数或微分。

---

再回顾书中内容：

因为不同李群下，对于加的定义不同，所以指数映射也有些不同，但是都在描述逼近微小扰动之后的旋转近似值。

2.56这种泰勒展开作为一种视角看到这个旋转更新问题。2.55是李群形势下描述

2.61跟2.53一样也在描述更新量问题。2.62跟2.54一样，表述了四元数形式下逼近微小扰动之后的旋转近似值。

***总结：***雅可比矩阵加微分方程：一阶泰勒展开近似表示。

***方法论：***如果一篇介绍这个概念的文章读不懂，那就多搜几篇，总有一篇符合你的胃口，直到弄懂为止。

雅可比矩阵配合微分方程：完成了对逼近局部值的一阶泰勒展开的近似。

为何要局部近似？因为这种多元向量值函数是一种隐函数，我们没法描述他的具体函数。只好做线性近似。是一种无奈之举。

· 但是只要足够逼近，那么就足够精确。

· 可微的意义：如果陡变，这种近似没有意义，不成立。所以这就是可微可导的实际物理意义。或者说几何意义。

六、拓展

主要搬运：雅可比矩阵几何意义的直观解释及应用-CSDN博客

原博主有个微信公众号：雅可比矩阵几何意义的直观解释及应用

Math1n51de 机器学习与数学

拓展一：雅可比行列式

多元向量值函数、导数（雅可比矩阵）、行列式、多元微积分等这些概念怎么就凑一起了呢？

当$m=n$时，雅可比矩阵为方阵，因此其行列式是有定义的，称为$f$的 雅可比行列式（Jacobian 行列式）。虽然它将$n^2$个数浓缩成了一个数，但它还算是精华的，包含了有关$f$的局部表现的重要信息。

从$uv-$平面到$xy-$平面，区域形状会变，面积也会变，每点处的局部面积缩放倍率就是该点处相应行列式的值。

矩阵行列式的几何意义，是指坐标变换前后面积的缩放倍数。

简单的看，上图中左边橙色小区域在坐标变换下变成右边的淡蓝色小区域，它的面积怎么变呢？因为站在每一点的局部，近似地看成一个仿射变换，而仿射变换对应一个矩阵，仿射变换对区域面积的具体缩放情况可以通过这个矩阵来估计。

对雅可比矩阵$J$作SVD分解：

$J=U\Sigma V^T$

其中，矩阵$U$和$V$都是正交矩阵，而是对角矩阵$\Sigma$。正交矩阵不改变向量的长度（向量的L2范数），因此雅可比矩阵$J$对区域面积的改变主要体现在中间的对角矩阵$\Sigma$，对角阵的对角线元素对应在各个坐标轴上的缩放倍数，它们的乘积就是整体面积的缩放倍数。而由柯西 - 比内公式可知：

$\left|J\right|=\left|U\right|\left|\Sigma \right|\left|V\right|=\left|\Sigma \right|$

再看坐标变换前后，对微小区域的面积可以作如下估计，

其中，$\mathbf{M}(\cdot )$是求区域的面积，${\Omega }_i$是定义域中的小区域，$\mathbf{g}({\Omega }_i)$是变换后得到的小区域。这里用橙色小区域的仿射变换来逼近表示淡蓝色小区域，

需要注意的是，这里实际的向量值函数是$\mathbf{g}$，记得用它代替上文中局部仿射变换逼近公式中的$\mathbf{f}$。

因此可得，

这就是雅可比行列式在多元微积分中的应用。可以直观地理解成，坐标变换引入了一个局部仿射变换，变换前后面积有缩放，缩放倍率正是雅可比矩阵的行列式。当然，这里只是直观地解释，并不是严格的证明。

拓展二：方程求根之牛顿法

1、一元数量函数

我们可以使用牛顿法来计算$f(x)=0$的根。假设根在$x_k$附近，为了找到更好的近似根$x_{k+1}$，我们考虑如下$\left(x_k,f\left(x_k\right)\right)$处的切线，

该切线与 x 轴的交点为$\left(x_k-\frac{f\left(x_k\right)}{f^{\prime }\left(x_k\right)},0\right)$，它揭示了比***$x_k$***更近的近似根。

因此，牛顿法的公式为

该方法的迭代过程如下图所示，实际操作中对初始值是有一定要求的。

图中显示了迭代过程，第一次求得x1之后，取f(x1)的值，在（x1, f(x1)）处继续做切线，相交于x轴于x2点，然后再在（x2,f(x2))点做切线。不断迭代。

这个就是牛顿法，求每个切线于x轴交点处（y=0）对应的曲线点处做切线。不断迭代循环逼近。

这么多年，终于被这个图教会了牛顿法。

核心：切线与x轴的交点是在不断逼近曲线零点的。

· 而这个交点就是通过迭代，不断靠近零点的。

· 每次迭代需要求解这个交点，而这个交点的求解与上一步的导数有关。

2、多元数量函数

对于多元数量函数$f:{\mathbb{R}}^n\mapsto \mathbb{R}$假设${\mathbf{x}}_k$接近根，那么下式

是在点${\mathbf{x}}_k$处对函数$f$的仿射逼近，此时可以沿着梯度所在方向找更近似的根，可以令

代入上式，从而求得根的更新公式为

3、多元向量值函数

对于多元向量值函数$f:{\mathbb{R}}^n\mapsto {\mathbb{R}}^m$

计算$\mathbf{f}(\mathbf{x})=0$的根。

稍微一想，你会发现这个其实是在求解一个方程组的解，这里的方程一般来说是非线性的，当然也可以包括特例：线性方程组。

如果是线性方程组，一般能一步到位；但如果是非线性的，往往不能一步到位，那就用迭代法一步一步逼近。假设${\mathbf{x}}_k$在根附近，该点处函数的仿射变换为，

其中，雅可比矩阵为，

如果$n=m$，此时牛顿法的迭代公式为，

上面公式里要计算雅可比矩阵的逆，一般并不会直接去求，而是通过求解下面的方程组，

然后更新

这里所谓的迭代式求根方法的主要思路就是从当前的近似解如何得到更近似的解。

牛顿法就是利用函数局部的仿射逼近来一步一步逼近根。

当然，具体实施中还需要处理初始值等一些细节问题。

另外，求解最优化问题也有牛顿法，而雅可比矩阵在最优化问题的求解方法中也会发挥作用，关于这些内容我们另外开篇再议。

总结：

可以用雅可比矩阵刻画一个多元向量值函数的局部，从而简化分析函数的局部性质。

雅可比矩阵是一条比较好的能够将多个内容串起来的线索。简单来看，它能将矩阵、仿射变换、行列式、特征值特征向量、导数、泰勒展开、微分方程组、方程求根、最优化甚至流形及其上的度量张量等等内容有机地牵扯起来。

---

​
​
​
​

一、高斯牛顿法详解

 拓展阅读：高斯牛顿法详解_gauss-newton算法步骤-CSDN博客

1、梯度下降法

 

无论一阶泰勒展开，还是二阶泰勒展开都是关于增量的方程。 

---

---

2、牛顿法

 这个自变量增量都是可求的。但是二阶求解复杂。因此为了简化有了下面的高斯牛顿法。不过只适用于最小二乘法。

---

---

3、高斯牛顿法

最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。而非目标函数展开。

记住这个增量方程中的(H(x_k)$。这里后面代码要用到。

缺点：当近似求解的增量过大时，算法无法收敛，我理解到是不是通俗说的SLAM飞了。

缺点：雅可比矩阵有时是奇异矩阵。 从而导致增量不稳定。

补充：来源《随手笔记——如何手写高斯牛顿法》

还是那句话：高斯牛顿法是对：最小二乘法展开的是后面的函数部分。将f(x)一阶泰勒展开（一阶就要带雅可比矩阵）。而非目标函数展开。是对小f(x)（每个样本即误差项）

下面这个图是讲最小二乘法的样式：

• 模型函数预测值与观察值（真实值）之间的偏差的二次方的加和为目标函数。这个目标函数等效上面提到的大F(x)。

• 而高斯牛顿法是将这个上面这个偏差项给展开了。是对这个偏差项进行了泰拉展开，而不是对目标函数。

最优化算法之高斯牛顿法

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
 
using namespace std;
using namespace Eigen;
 
int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器
 
  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }
 
  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
 
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {
 
    Matrix3d H = Matrix3d::Zero();             // 黑森(海塞)矩阵：Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;
 
    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc
 
      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;
 
      cost += error * error;
    }
 
    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
 
    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }
 
    ae += dx[0];
    be += dx[1];
    ce += dx[2];
 
    lastCost = cost;
 
    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }
 
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
 
  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

手写高斯牛顿法-CSDN博客

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
 
using namespace std;
using namespace Eigen;
 
int main() {
    //设定曲线真实参数
    double ar = 1.0, br = 2.0, cr = 1.0;
    //给定曲线参数优化初始估计值
    double ae = 2.0, be = -1.0, ce = 5.0;
    //设定数据点个数
    int N = 100;
    //设定噪声服从的正态分布的sigma值
    double w_sigma = 1.0;
    //计算sigma的倒数，之后用于误差归一化
    double inv_sigma = 1.0 / w_sigma;
    //OpenCV随机数产生器
    cv::RNG rng;
 
    //初始化数据容器，容器内元素类型为double
    vector<double> x_data, y_data;
    //生成N个数据点
    for (int i=0; i < N; ++i){
        //x在0-1之间均匀取100个值
        double x = i / 100.0;
        x_data.push_back(x);
        //y用真实函数生成再加上高斯噪声
        y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma * w_sigma));
    }
 
    //开始高斯牛顿迭代
    //设定迭代次数
    int iterations = 100;
    //本次迭代和上次迭代的cost
    double cost = 0, lastcost = 0;
 
    //开始及时，当前时间点存储到t1中
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
 
    //牛顿高斯算法迭代iterations次
    for (int iter = 0; iter < iterations; ++iter) {
 
 
 
        //初始化H矩阵，b矩阵,雅克比矩阵J和cost
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;
 
        //对N个数据点进行处理，列出总的增量方程，计算初始误差
        for (int i = 0; i < N; ++i) {
            double xi = x_data[i], yi = y_data[i];
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            //计算雅克比矩阵在该点取值
            Vector3d J;
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);   // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);   // de/dc
 
            H += inv_sigma * inv_sigma * J * J.transpose();   //这里除以sigma是归一化
            b += -inv_sigma * inv_sigma * error * J;
 
            cost += error * error;
        }
 
        //求解线性方程Hx=b
        Vector3d dx = H.ldlt().solve(b);
        //如果方程无解，那么dx[0]是非法字符nan，退出迭代
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }
 
        //如果本次迭代误差大于上次误差，算法结束，退出迭代
        if(iter > 0 && cost >= lastcost){
            cout << "cost:" << cost << ">=" << lastcost << ",break." << endl;
            break;
        }
 
        //进行估计参数的增量更新，存储本次代价
        ae += dx[0];
        be += dx[1];
        ce += dx[2];
 
        lastcost = cost;
        //输出本次迭代信息
        cout << "total cost:" << cost << ",\t\tupdate:" << dx.transpose() << "\t\testimatec:" << ae << "," << be <<
        "," << ce << endl;
 
    }
    //及时结束，获取当前时间赋给t2
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //计算算法耗时并输出
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
 
    //输出最终算法迭代结果
    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}
 

cmake_minimum_required(VERSION 3.15)
project(GuassNewton)
 
set(CMAKE_CXX_STANDARD 14)
 
#OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
 
#Eigen
include_directories("/usr/include/eigen3")
 
add_executable(GuassNewton main.cpp)
target_link_libraries(GuassNewton ${OpenCV_LIBS})

---

---

 4、列文伯格-马夸尔特方法

 因为高斯牛顿更新时增量可能会不稳定，甚至太大。所以为了使增量的更加稳定可靠，对其做了限制，增加了置信域。

---

---

5、拟牛顿法

为了解决牛顿法中海森矩阵H计算复杂的问题，拟牛顿法提供了另外一种解决思路：

通过使用不含有二阶导数的矩阵U代替牛顿法中的H，根据矩阵U构造的不同，具有不同的拟牛顿法。

1、拟牛顿法的基本原理

2、DFP

为了方便区分，下面把U称作D（表示DFP）

1）DFP结果

DFP算法的问题在于在求取增量的时候D矩阵仍要求逆

3、BFGS

2)BFGS算步骤

4、L-BFGS

1）L-BFGS原理

2）L-BFGS应用

 

因此，L-BFGS的算法流程如下：
 

---

​
​
​
​ 卡尔曼滤波器

为了研究卡尔曼，我阅读了大量博文。不敢说完全吃透，但是在做一件什么事，可以通过下面这文章来理解，我读了不下五遍。并整理标准重点，添加自己的一些见解。

自动驾驶传感器融合算法 - 自动驾驶汽车中的激光雷达和雷达

自动驾驶传感器融合算法-自动驾驶汽车中的激光雷达和雷达

来源：

原创 HITJACKJU [机器人规划与控制研究所](javascript:void(0)😉 2024-06-26 17:32 美国

在本文中，我们将深入探讨 LiDAR 和 RADAR 之间的传感器融合。这两种传感器在自动驾驶和许多其他机器人应用中被广泛使用。我们将首先简要介绍这两种传感器，然后介绍我们使用的融合算法及其特性。

顺便问一下，什么是传感器融合？在感知模块中，即观察环境（道路线、交通标志、建筑物、行人等），我们需要使用多个传感器。这可以增加冗余度、确定性，或者利用多个传感器的优势并创建多个用例。这创建了一个我们称之为传感器融合的领域。

例如，使用相机可以让我们看到交通信号灯的颜色。它是分类、车道线检测等的完美工具……使用LiDAR非常适合 SLAM（同步定位和地图构建）和深度 估计——即估计任何物体的精确距离。最后，RADAR具有一种可以测量物体 速度并给你开超速罚单的特定技术！

在本文中，我们将学习融合 LiDAR 和 RADAR，从而利用LiDAR 技术估计距离并以 3D 方式观察世界，并利用 RADAR估计速度的能力。

一、用于 LiDAR 雷达融合的传感器数据

在考虑任何传感器融合任务之前，我们必须做两件事：

1. 选择多个传感器进行合并——并定义一个明确的目标。

2. 研究两个传感器来确定“如何”将它们融合。

让我们花一点时间来回顾一下不同的传感器…

1、雷达Radar（无线电探测和测距）

雷达发射无线电波来探测几米（约 150 米）范围内的物体。多年来，雷达一直安装在我们的汽车上，用于探测盲区中的车辆并避免碰撞。

与其他传感器计算两次测量之间的位置差异不同，雷达利用多普勒效应，通过测量车辆向我们移动或远离时下一波频率的变化。这称为径向速度。

雷达可以直接估算速度。

因此，它们在移动物体上的表现比在静态物体上的表现更好。

它的分辨率较低 ，可以知道被检测物体的位置和速度。另一方面，它很难确定被检测的物体是什么。

2、LiDAR（光检测和测距）

LiDAR 使用红外传感器来确定与物体的距离。旋转系统可以发送波并测量波返回所需的时间。这使得可以生成传感器周围环境的点云。它每秒可以生成大约 200 万个点。点云具有不同的 3D 形状，因此可以借助 Lidar 对物体进行分类。

它具有良好的范围（100 至 300 米），并且可以准确估计周围物体的位置。但是它的尺寸很笨重，有些人可能会把它看作拐杖。更不用说，它的价格（1,000-50,000 美元）多年来一直很高，而且仍然远远高于相机（500 美元）或雷达（300 美元） 的平均价格 。

现在我们对雷达和激光雷达有了很好的了解。让我们看看如何将它们融合！

二、使用哪种类型的数据融合？

正如我在有关传感器融合的文章中所解释的那样，我们有 3 种类型的传感器融合分类：

• 低级传感器融合- 融合 RAW 数据

• 中级传感器融合- 融合物体

• 高级传感器融合——融合物体及其时间位置（跟踪）

在本文中，我们将重点介绍中级传感器融合（后期融合），并了解如何将 RADAR 的输出与 LiDAR的输出相结合。也可以将 RADAR的原始数据进行组合，但RADAR的噪声非常大。因此，所涉及的后期处理级别使我们使用后期融合。

三、用于LiDAR雷达融合的传感器融合算法

到目前为止，我们了解到：

• 我们将雷达Radar和激光雷达Lidar合作

• 我们将融合结果，而不是原始数据。

那我们该如何处理呢？

1、使用卡尔曼滤波器进行传感器融合

用于合并数据的算法称为卡尔曼滤波器。

卡尔曼滤波器是数据融合中最流行的算法之一。它由鲁道夫·卡尔曼于 1960 年发明，现在用于我们的手机或卫星的导航和跟踪。该滤波器最著名的用途是在阿波罗11号任务中，该任务将宇航人员送至月球。

在本文中，您将了解卡尔曼滤波器的工作原理，以及如何使用卡尔曼滤波器合并来自两个传感器的数据。使用的示例是LiDAR和RADAR，但实际上我们可以用任何传感器来实现这一点，正如您将看到的。

2、何时使用卡尔曼滤波器？

卡尔曼滤波器可用于数据融合，以估计动态系统（随时间发展）的现在（过滤）、过去（平滑）或未来（预测）的状态。

自动驾驶汽车中嵌入的传感器发出的测量结果有时不完整且有噪声。传感器的不准确性（也称为噪声）是一个非常重要的问题，可以通过卡尔曼滤波器来处理。让我告诉你如何解决。

卡尔曼滤波器用于估计系统的状态，表示为 x。该状态向量由位置 p 和速度 v 组成。


在每个估计中，我们都关联一个不确定性度量 P。使用不确定性度量非常好，因为我们可以考虑到LiDAR在我们的测量中比RADAR更准确的事实！（前置信息、预测评估提示）（就理解为误差）

通过进行数据融合，我们考虑传感器噪声和输出。

举个例子，看一下这两个LiDAR融合在一起观察行人的情况。

两个传感器都同样确定，但我们可以看到，如果它们不确定的话，情况会发生怎样的变化。

与只有一个传感器的情况相比，考虑多个传感器及其不确定性可以帮助我们更好地了解行人的位置。

现在，让我们看看如何在雷达/激光雷达融合中实际使用卡尔曼滤波器。首先，我们需要了解传感器融合算法（如卡尔曼滤波器）表示估计值的方式；使用高斯函数。

3、高斯

状态和不确定性用高斯表示。

高斯是一个连续函数，其面积为 1。正如您在这里看到的，高斯是一个以我们所谓的平均值为中心的公式，协方差有助于理解我们有多“确定”。

我们用均值 μ 表示状态，用方差 σ² 表示不确定性。

方差越大，不确定性越大。

高斯可以估计系统状态和不确定性的概率。

我们采用的是正态分布的概率。卡尔曼滤波器是单峰的：这意味着我们每次都有一个峰值来估计系统的状态。

换句话说：障碍物并不是在 90% 距离 10 米处和 70% 距离 8 米处；而是在 98% 距离 9.7 米处或没有障碍物处。

卡尔曼滤波器是连续的单峰函数。

到目前为止，我们已经了解了卡尔曼滤波器如何在表示过程中发挥作用，但在传感器融合过程中却没有发挥作用。现在，让我们通过理解一个关键概念来考虑传感器融合：贝叶斯滤波。

4、贝叶斯过滤

卡尔曼滤波器是贝叶斯滤波器的一种实现，即反复在预测和更新之间交替。

• 预测：我们利用估计的状态来预测未来的状态和不确定性。

• 更新：我们使用传感器的观测来修正预测并获得更准确的估计。

传感器融合的具体实现如下：传感器数据到达；我们更新正在跟踪的行人的估计位置，并预测下一个行人的位置。接下来，新的传感器数据到达，我们更新行人的位置，评估预测的准确程度，并据此预测下一个行人的位置。

⚠️要对卡尔曼滤波器进行估计，只需要当前观测值和先前预测值。不需要测量历史。这不是机器学习，而是人工智能。因此，此工具很轻便，并且会随着时间的推移而改进。

---

让我们回顾一下，分析一下迄今为止所学到的一切：

1. 我们正在进行LiDAR和RADAR之间的传感器融合

2. 但只有它们各自的输出（后期融合）

3. 为此，我们使用卡尔曼滤波器

4. 哪些是单峰的，并使用高斯来表示状态和不确定性

5. 并使用预测/更新循环（也称为贝叶斯过滤）进行工作。

现在，让我们看看它的内部工作原理。

个人总结一波：这个预测和更新是重点，得明白这个过程。

1、初始化：读取第一帧数据，估计当前时刻的状态，并预测下一时刻也就是第二帧数据时的状态。

2、读取第二帧数据：基于此数据，我更新当前时刻的状态（这个状态是上一时刻预测的），生成新的当前状态的估计值，并预测第三帧数据的状态。换言之：数据来之后，我先更新上一时刻对此时刻的预测，生成对此刻状态新的估计值，并对下一时刻进行预测。（或者说基于上一帧的预测，结合传感器的读数，估计出当前的最优估计状态值。并生成下一时刻的预测）。

3、不断循环第2步。

PS：后文称：读取第n帧数据为：测量measurement。

四、卡尔曼滤波器背后的数学

卡尔曼滤波器背后的数学原理是由矩阵的加法和乘法组成的。我们有两个阶段：预测和更新。

在本节中，我将概述卡尔曼滤波器背后的公式。如果您想进一步了解并真正掌握这些内容，请参加我的课程学习卡尔曼滤波器：默默为未来提供动力的隐藏算法。在那里，您将了解卡尔曼滤波器的工作原理，了解数学原理，并从头开始为不同的用例编写自己的卡尔曼滤波器。

1、预测公式

我们的预测包括根据时间 t 时的先前状态 x 和 P 估计时间 t+1 时的状态 x’ 和不确定性 P’。

$x^{\prime }=Fx+u$

$ {P}’ = FPF^{T} + Q$

因此，我们只有两个公式来估计 x 和 P。其他值是：

• F：从 t 到 t+1 的转移矩阵

• u：噪声

• Q：包含噪声的协方差矩阵

👉如果我们简化事情——我们可以说我们的新位置 x’ 等于前一个位置 x，乘以矩阵 F。这个矩阵 F 称为我们的运动模型。

这是一个关于它的外观和使用方法的简短示例。

$$\begin{gathered} x^{\prime }=Fx \\ {x}^{\prime }=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}p\\ v\end{array}\right] \\ {x}^{\prime }=\left[\begin{array}{c}p+v\Delta t\\ v\end{array}\right] \end{gathered}$$

如您所见，F 只是一个矩阵，描述我们如何从步骤 t 移动到步骤 t+1。

这里，我们有：

• 位置（t+1）=位置（t）+速度（t）*时间

• 速度（t+1）=速度（t）

换句话说，我们考虑恒定速度。

我们的预测方程旨在转换两个时间帧之间的运动。我们可以让 F 矩阵实现不同的运动模型，例如恒定转弯速率、恒定速度、恒定加速度……

这就是我们预测下一个位置的方法 - F 是用于平移运动的矩阵。现在，让我们看看更新公式。

2、更新公式

在更新步骤中，我们要调整我们的位置，并纠正我们下一步的预测方式。

$$\begin{gathered} y=z-H{x}^{\prime } \\ S=H{P}^{\prime }{H}^T+R \\ K=P^{\prime }{H}^T{S}^{-1} \end{gathered}$$

$$\begin{gathered} x=x^{\prime }+Ky \\ P=(I-KH)P^{\prime } \end{gathered}$$

让我们看一看它是如何工作的——我们有时间 t+1 的 x’（预测状态）和 P’（预测协方差）。

现在，我们收到了新的传感器数据并可以确认我们是否接近。

• y 是实际测量值与我们的预测之间的差异——这是预测误差！（这个差值，结合卡尔曼增益更新最优状态估计）

这个预测误差后面《学习笔记十》中，还会用到。这个就是ESKF中的误差状态。

• 其他矩阵用于考虑传感器噪声（R）、估计系统误差（S）和卡尔曼增益（K）

• 最后一个值卡尔曼增益K介于 0 和 1 之间，有助于决定我们是否应该更信任预测或测量。

• 所有这些都导致计算一个新的 x 和一个新的 P。

更新阶段可以估计出比测量和预测更接近现实的 x 和 P。

经过几个循环后，卡尔曼滤波器将会收敛，做出越来越准确的预测。

在回顾并了解全局之前，让我们先考虑以下有关贝叶斯概率的内容。

说大事专用起始分割符===

上面提到卡尔曼增益K是一个介于0和1之间的数值。

$x=x^{\prime }+Ky$

其中这个y：是运动方程对当前状态的预测值，与观测方程对当前的观测值之间的差，上面称为预测误差。（ESKF中的误差状态ES）

换句话说：对当前状态量：

• 运动方程有个预测值

• 观测方程有个测量值

• 现在这俩值摆在这里，我们该信谁？（误差/偏差/不确定性/协方差）。我们要来确定当前状态的最优估计。

• 真相往往可能存在于这两者之间。中庸之道。到底倾向于多少呢？这个预测误差y，（下一篇卡尔曼滤波器二中提到的温度差）把这两者之间的差异做了一个划分，而卡尔曼增益K来决定，最优估计到底应该倾向于两者中的哪一个。

这个差异操作，相当于更新，我们下一篇文章，讲卡尔曼滤波器二中，还要提及。

说大事专用终止分割符===

我个人有个疑问：我们总是假设真值在两个测量值中间。有没有可能两个测量值都属于测度不足的情况。比如预估温度。真实的温度要比测量的温度高很多。两个或者多个传感器都是测量不到真实值呢？都低于真实值呢？这个偏差如何做？个人疑问：有读者有答案，跪求留言指导。

3、先验/后验和贝叶斯过滤

该图显示了贝叶斯过滤器内部的最终数学。条件概率

我们想要估计的就是后验概率。从数学上讲，它等于先验概率（预测）乘以似然概率（测量）。

个人总结一波：

• P(A)：先验概率：预测。基于A对B发生的条件概率

• P(B|A)：似然概率：测量。 是在A为真的情况下观测到B的概率，这里可以理解为观测的准确性。

• P(B)：参照标准：B发生的概率。

• P(A|B)：已知B发生的条件下，反推A在B发生之后的条件概率。成为后验概率。

• 我们原来是拿A预测B的，此时AB都发生了，即得到了P(AB)，也即：既有预测数据又有测量数据。那么依托测量数据的条件下，更新A发生的概率。

• 再言之：我本来想法是依托T1时刻预测T2时刻的状态。这个预测就是先验概率。等我在T2时刻拿到测量数据之后，也就是这个测量读数成为似然概率的事实，此时对于T2时刻来说，预测和测量都有了。事件都发生了。基于此事实上，我可以更新T1时刻的状态，使我的T1时刻的状态估计的更加准确。

• 人话就是：线性数学模型算出预测值+传感测量值=更准确的测量值。

将其翻译成高斯如下：

如你所见，更新（后验）始终是最佳估计。其次是测量和预测，这自然是三者中最不确定的。

此时，您可能会想“好吧…这很酷，但是 Jeremy，您并没有谈论激光雷达和雷达…您只是以非常笼统的方式谈论了传感器融合。”

你说得对，那我们就进入正题吧。

五、雷达/激光雷达传感器融合流程

流程如下：每次传感器到来时，我们都会运行一个新的预测+更新循环。

• 每次预测都会降低确定性，

• 每次更新都会增加确定性。

• 但最终，两者结合/融合可以提供更多数据，并获得更好的总体结果。

需要理解的是：我们始终使用相同的状态——我们的状态（位置、速度）估计。

1、在雷达 LiDAR 融合中使用卡尔曼滤波器

使用LiDAR时，我们有“笛卡尔”线性值。我们的数学公式全部用 y = ax + b 类型的线性函数实现。

另一方面，当我们使用雷达Radar时，数据不是线性的。该传感器通过三个指标来观察世界：

• 𝞺（rho）：到被跟踪物体的距离。

• φ（phi）：x 轴和物体之间的角度。

• 𝞺 ̇ (rhodot)：𝞺 的变化，导致径向速度。

由于包含了角度 φ，这三个值使得我们的测量结果呈非线性。

意思是说，当使用雷达Radar时，我刚才谈到的一切都是错误的，无法起作用。

2、非线性卡尔曼滤波器

这里的一切都将我们引向这一点——世界是非线性的。并非所有事物都完美地沿直线移动。传感器的工作方式并不相同。因此，我们必须使用两种特定类型的卡尔曼滤波器：

• 扩展卡尔曼滤波器：EKF

• 无迹卡尔曼滤波器

这些过滤器将首先“线性化”非线性数据，以便它可以在经典过滤器中工作。

• 在扩展版本中，我们使用雅可比矩阵和泰勒级数对数据进行线性化。

• 在无迹版本中，我们使用 Sigma 点预测。

3、最终传感器融合流程

考虑到这一点，这是我们最终的传感器融合流程。

LiDAR会定期更新，而RADAR则需要进行线性化处理。这个循环不断重复。我们永远不会将数据融合在一起，而总是一个接一个地进行。

RMSE 值是均方根误差测量，即我们的预测与现实之间的误差。（原文有图）

👉无迹卡尔曼过滤器的误差比扩展过滤器的误差更低，因为这种技术更有效。

因此，我们有两种技术——对于这个特定的用例，一种似乎比另一种更好。

六、结论

传感器融合是自动驾驶汽车中最重要的主题之一。传感器融合算法可让车辆准确了解障碍物的数量，并估计它们的位置和行驶速度。这至关重要。

它们总是依赖于我们拥有的传感器：卡尔曼滤波器不是一个系统的答案。

现在每天都会使用多个传感器。它用于物体检测、定位、计算机视觉和跟踪。

---

相关文章

1. 自动驾驶算法——使用扩展卡尔曼滤波算法实现传感器融合和物体跟踪。（有代码）

2. 关于卡尔曼及卡尔曼增益的理解【精】

3. 自动驾驶-序惯卡尔曼滤波【附Github源代码】

---

---

七、补充阅读1：variance与协方差

User: variance

Kimi: 方差（Variance）是统计学中衡量数据离散程度的一个重要指标，它描述了数据点与均值之间的差异。具体来说，方差是每个数据点与均值差的平方的平均值。

在概率论中，方差用来衡量随机变量与其期望值（均值）之间的偏离程度。方差通常用希腊字母σ²（sigma squared）表示。方差的计算公式在离散型随机变量和连续型随机变量中有所不同，但基本原理是相同的。方差不会是负数，因为它是平方的结果。如果一个数据集的方差为零，这意味着所有数据点都相同，没有变异性。

在实际应用中，方差有时因为单位平方而难以直观理解，因此经常使用方差的平方根——标准差（Standard Deviation），来描述数据的离散程度。标准差与原始数据具有相同的单位，更易于解释和比较。

方差的概念最早由罗纳德·费雪（Ronald Fisher）提出，并在统计学中得到广泛应用。它不仅用于**描述单个数据集的变异性，**还可以用于衡量不同数据集之间的差异。

例如，在经济学、金融学和其他领域，方差和标准差常用于衡量风险和不确定性。()

方差的计算方法在总体数据和样本数据中有所不同。

• 总体方差的计算公式是所有数据点与总体均值差的平方和除以总体大小。

• 而样本方差的计算则是样本中每个数据点与样本均值差的平方和除以样本大小减一（(N-1)），这是一种无偏估计，用于估计总体方差。

方差在数学上也有其扩展形式，例如对于随机向量，方差可以表示为协方差矩阵，这在多维数据分析中非常重要。此外，方差在概率论中也有其特定的定义和性质，例如在连续分布中，方差是通过积分来计算的。

总的来说，方差是描述数据分布特性的关键统计量，它在数据分析、风险评估和科学研究中都有着广泛的应用。

---

---

对于这个协方差，我必须推荐一篇文章：概率论—协方差/相关系数/协方差矩阵

 

 总结一句：协方差矩阵，可以表述变换的旋转方向，以及变换的尺度，或者说包含了变换的权重信息。

---

---

八、协方差其意义：

度量各个维度偏离其均值的程度（均值期望预测误差有多大）。协方差的值如果为正值，则说明两者是正相关的(从协方差可以引出“相关系数”的定义)，结果为负值就说明负相关的，如果为0，也是就是统计上说的“相互独立”。

终于明白协方差的意义了-CSDN博客

协方差的意义_协方差网络-CSDN博客

 在概率论中，两个随机变量 X 与 Y 之间相互关系，大致有下列3种情况：

 

当 X, Y 的联合分布像上图那样时，我们可以看出，大致上有： X 越大  Y 也越大， X 越小  Y 也越小，这种情况，我们称为“正相关”。

当X, Y 的联合分布像上图那样时，我们可以看出，大致上有：X 越大Y 反而越小，X 越小 Y 反而越大，这种情况，我们称为“负相关”。

当X, Y  的联合分布像上图那样时，我们可以看出：既不是X  越大Y 也越大，也不是 X 越大 Y 反而越小，这种情况我们称为“ 不相关”。

怎样将这3种相关情况，用一个简单的数字表达出来呢？

在图中的区域（1）中，有 X>EX ，Y-EY>0 ，所以(X-EX)(Y-EY)>0；

在图中的区域（2）中，有 X<EX ，Y-EY>0 ，所以(X-EX)(Y-EY)<0；

在图中的区域（3）中，有 X<EX ，Y-EY<0 ，所以(X-EX)(Y-EY)>0；

在图中的区域（4）中，有 X>EX ，Y-EY<0 ，所以(X-EX)(Y-EY)<0。

当X与Y正相关时，它们的分布大部分在区域（1）和（3）中，小部分在区域（2）和（4）中，所以平均来说，有E(X-EX)(Y-EY)>0。

当X与Y负相关时，它们的分布大部分在区域（2）和（4）中，小部分在区域（1）和（3）中，所以平均来说，有(X-EX)(Y-EY)<0 **。

当X与Y不相关时，它们在区域（1）和（3）中的分布，与在区域（2）和（4）中的分布几乎一样多，所以平均来说，有(X-EX)(Y-EY)=0。

所以，我们可以定义一个表示X, Y 相互关系的数字特征，也就是 协方差

cov(X, Y) = E(X-EX)(Y-EY) 。

当cov(X, Y)>0时，表明X与Y正相关；

当cov(X, Y)<0时，表明X与Y负相关；

当cov(X, Y)=0时，表明X与Y不相关。

这就是协方差的意义。

---

高翔【自动驾驶与机器人中的SLAM技术】学习笔记（六）卡尔曼滤波器二：图解卡尔曼滤波器；卡尔曼滤波器公式理解；面试答法；噪声协方差（不确定性）如何更新；OpenCV中卡尔曼实例；卡尔曼滤波器代码实例；

逍遥郎wj已于 2024-12-06 15:07:12 修改

阅读量1.1k 收藏 11

点赞数 30

分类专栏： 高翔书《自动驾驶与机器人中的SLAM技术》学习笔记 文章标签： 自动驾驶 机器人 卡尔曼滤波

版权

上一篇卡尔曼滤波器一中，从整体上认识了，卡尔曼滤波器整体是在做一件什么事。

知道了，协方差就可以理解为偏差，或者误差。

这一篇主要讲卡尔曼滤波器中的公式，理解公式，就能知道如何实现卡尔曼滤波器。

上一篇：卡尔曼滤波器在做一件什么事，这一篇，卡尔曼滤波器怎么做。

---

博文一：

为了搞懂卡尔曼我按照吃饼原理：读了大量的博文。首先让我有点理解的感觉源自：百度百科中的例子。这个可能有点反常识，但确实是百度百科，让我有点理解的感觉。

为了弄懂（理解一点），我连百度百科都没放过。但是人家写的确实不错。

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

几个重点：

• 线性系统
• 系统状态最优估计
• 最优估计也可以看做是滤波过程：滤除系统中的噪声和干扰的影响。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

性质：

①卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。

②任何一组观测数据都无助于消除x(t)的确定性。增益K(t)也同样地与观测数据无关。

③当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计，也就是最小方差估计。

案例：

卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。

人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对目标当前位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

测量值有噪声，通过滤波设法去掉噪声的影响。

受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。

使估计值尽可能准确地接近真实值，这就是*最优估计*。

真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。

对于理解卡尔曼滤波器最有帮助的是它文中举得例子：

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的（假设），也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设预测值是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的**协方差(covariance)**来判断。因为Kg=5^2/(52+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：

23+0.61*(25-23)=24.22度。

可以看出，因为温度计的协方差(covariance)比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*5²⁾0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

公式理解

我个人补充两个公式，对比上面的公式：

23+0.61*(25-23)=24.22度。0.61 = 5^2/(52+4^2) 公式1

25+0.39*(23-25)=24.22度。0.39 = 4^2/(52+4^2) 公式2

这个25-23可以理解为估计值和观测值的差值。

预测值：23度 偏差为5度。 偏差占总偏差权重0.61 = 5^2/(52+4^2)

测量值：25度 偏差为4度。 偏差占总偏差权重0.39 = 4^2/(52+4^2)

预测值	23度	偏差为5度	偏差占总偏差权重:0.61 = 52/(52+4^2)
测量值	25度	偏差为4度	偏差占总偏差权重:0.39 = 42/(52+4^2)

我试图找到一个合适的协方差解释：

23	0.61
25	0.39

$$\begin{gathered} \left|\begin{array}{cc}23& 0.61\\ 25& 0.39\end{array}\right|=23\ast 0.39+25\ast 0.61=24.22 \\ 23\ast 0.39+25\ast 0.61 \\ =23\ast (1-0.61)+25\ast 0.61=23+0.61\ast (25-23) \\ =23\ast 0.39+25\ast (1-0.39)=25+0.39\ast (23-25) \\ =24.22 \end{gathered}$$

按照另外一种（卡尔曼滤波示例）解释方式：

换一种理解方式：误差占比权重越大，越不可信。

那么可信度为 = 1 - 误差占比权重。为置信度。所以按照置信度来给上面的两个温度值赋予可信度。分别为

	温度值（溶度值/甜度）	置信度（含水量/溶剂）
预测值	23	0.39
测量值	25	0.61

我自己创造一个词：指导预测的参考价值（溶质值）：23 * 0.39 + 25*0.61

最优估计值：溶质值/总含水量 = （23 * 0.39 + 25*0.61）/ (0.39+0.61) = 24.22

因为我们是做过归一化的，所以除法可以省去。

这个公式$T = (0.9 * 30 + 0.7 * 25) / (0.9 + 0.7) \approx 27.8 \$中的除法，也是为了做了归一化，确定置信占比。

计算方法：

结论1：

至此，两种计算方法：

• 如果是

  误差

  ，那么按照：

  • 最优估计 = 预测值 + 预测误差占总误差比重 * （观察值 - 预测值）
  • 或者交叉相乘：最优估计 = 预测值 * 测量误差占比 + 观测值 * 预测误差占比
  • 总误差 = 预测误差 + 测量误差；

• 如果是置信度：那么按照：溶质，溶剂计算方法提炼有效信息。

结论2：

对于协方差的理解：与其说是置信度，不如说是误差才对。

---

回到高翔书中：

我们还是不可以脱离高翔的书来讲卡尔曼滤波器。

• R理解为运动方程的误差（协方差），或者说预测值的偏差。
• Q理解为观察方程的误差（协方差），或者说测量值的偏差。
• 都是线性函数。也就是线性变换。或者说可以近似转化为线性函数。泰勒一阶展开。

黄金五公式

状态估计：状态量为：位姿均值（最优期望）和偏差（协方差矩阵）。

我们先看公式（2.108）这个是修正公式或者说是更新公式。

求解的是最优估计值：也就是上面我们反复提及的24.22度。

$x_k=x_{k,pred}+K_k(z_k-C_k{x}_{k,pred})$

最优估计值 = 预测值 + 卡尔曼增益 *（观察值 - 预测值）

理解为：最优估计为：预测值 + 卡尔曼增益化之后的（观察值和预测值之间的差值）

这里还需要补充一个观测矩阵$C_k$。参照公式（2.105)中的观测矩阵。

$$\begin{gathered} x_k=A_k{x}_{k-1}+u_k+w_k \\ {z}_k=C_k{x}_k+v_k \end{gathered}$$

所以更准确的描述为：

最优估计值 = 预测值 + 卡尔曼增益 *（观察值 - 观测矩阵 * 预测值）

这个观测矩阵：用来表示状态量对测量量的增益。

一图看懂卡尔曼滤波器

更准确描述为：预测值转化到观察值或者测量值视域（度量角度/度量空间）之后的与实际测量值之间的差异。拔河比赛中的力量差值。

这个卡尔曼增益作为一个权重，将差异化数据划分给预测值，使其根据权重来决定或调整与最优值之间的距离。最优值应该靠近哪边。

黄金五公式详解

状态修正公式

对于这个状态修正公式，我们做一点变换：展开看一看。

$$\begin{gathered} x_k=x_{k,pred}+K_k(z_k-C_k{x}_{k,pred}) \\ =x_{k,pred}+K_k\cdot z_k-K_k{C}_k\cdot x_{k,pred} \\ =K_k\cdot z_k+(I-K_k{C}_k)\cdot x_{k,pred} \end{gathered}$$

与公式（2.108）中的估计协方差矩阵更新公式对比一下。

$P_k=(I-K_k{C}_k)P_{k,pred}$

预估误差协方差，在卡尔曼滤波状态更新中这个值会自动调整，所以初始值可以设置为1，或者其他非零值。

这个协方差前面提到，可以看做一个描述误差程度的参考值。

公式（2.108）中，变形后：

$$\begin{gathered} x_k=K_k\cdot z_k+(I-K_k{C}_k)\cdot x_{k,pred} \\ {P}_k=(I-K_k{C}_k)\cdot P_{k,pred} \end{gathered}$$

两个更新中，都用到了$(I-K_k{C}_k)$。到这边如果不懂，可以先看博文二：这里我是后补充的。这个量我们可以称呼为：测量误差占比。而卡尔曼增益可以看做预测误差占比。

最优估计$x_k=$ 预测误差占比 * 观测值 + 测量误差占比 * 预测值

交叉相乘。

---

把五个公式捋一捋。

预测公式/递推公式

先看最简单的预测公式，也就是递推公式。公式（2.106）

$x_{k,pred}=A_k{x}_{k-1}+u_k,P_{k,pred}=A_k{P}_{k-1}{A}_k^T+R_k$

这个主要依赖于公式（2.105）中的$x_k=A_k{x}_{k-1}+u_k+w_k$

在下面这个博文中，有关于矩阵求法的实例。

卡尔曼滤波示例https://zhuanlan.zhihu.com/p/29191795

• 为系统的转移矩阵。因此，我们可以说，x如何变，P跟着一起变就可以。所以才有了，（2.106）中P跟着一起与A相乘。
• $u_k$：系统控制量。
• $R_k$：是系统估计的误差，也就是说你的预测系统的误差。
• $x_{k,pred}$：是用上一刻系统的最优估计状态，来对本时刻状态进行预测的值。
• $P_{k,pred}$：预测的这个值的协方差有多大。基于上一时刻系统最优估计的协方差经过转移矩阵加权之后加上预测系统本身的误差（偏差)。

卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。

---

---

博文二：

''说人话"系列之卡尔曼滤波https://zhuanlan.zhihu.com/p/93011093

注意：这篇博文中的R和Q与上方的R和Q刚好对调了。

要较为准确的预测下次的考试成绩，关键是确定卡尔曼增益$K_k$的值是多少，而卡尔曼滤波要做的工作，也正是不断地修正$K_k$的值好让我们能更为准确地预测下一个值。

卡尔曼滤波的主要作用，主要还是找一个“确定一个参考价值”来预测下一次的值，是一个“算命”用的公式。

递推公式：

递推公式中的上文中的$R_k$，此处是上图公式中Q是一个经验值，说白点就是你自己看着办，$R_k$或者说Q越小表示越信任预测值，为0的话表示完全相信预测值，越大表示越相信测量值。

Q是系统的估计误差，Q越大，说明方差越大，估计越不准确，置信度越小。

如何观测系统考核的是温度或速度恒定，那么此处的A为1。 B为零。

此处公式中还有个$u_{k-1}$，这个量是系统控制量。 运动系统中有加速运动，或者温度系统中有加热器或者散热器会出现这个量。匀速/恒温则不会出现。

$$\begin{gathered} X_k=X_{k-1}, \\ {P}_{\bar{k}}=P_{\overline{k-1}}+Q \end{gathered}$$

• $P_{\bar{k}}$是预估误差协方差，这个系统预测值到底可信度是多少。这个$P_{\bar{k}}$你只需要初始值设为1剩下的你就不用管了,不然我们要卡尔曼滤波来干什么,卡尔曼滤波的过程也就是$P_{\bar{k}}$不断调整的过程。
• 𝑄 是一个经验值，说白点就是你自己看着办，𝑄 越小表示越信任预测值，为0的话表示完全相信预测值，越大表示越相信测量值，在现在这个情况下，因为我100%相信房间里的温度短时间是不会变的，因此在这里我完全相信下一次房间的温度还是不会变的，因此这里 𝑄 直接取0就行了。

$${}_{\bar{k}}=P_{\overline{k-1}}$$

更新公式：

• H：是状态量到观测量的转换矩阵，在恒温或者恒速状态下这个值为1。

所以如果H为1，那么上面的公式可以简化为：

$$\begin{gathered} {K}_k=\frac{P_{\bar{k}}}{P_{\bar{k}}+R} \\ {X}_k=X_{\bar{k}}+K_k(Z_k-X_{\bar{k}}) \\ {P}_k=(I-K_k)P_{\bar{k}} \end{gathered}$$

• $X_k$：是我们的状态估计值,也就是卡尔曼滤波后我们"猜"出来的温度值,它的初值你可以直接设为0。
• $P_{\bar{k}}$：是预估误差协方差,理不理解无所谓了,反正它的初值你可以直接设为1或者随你喜欢的一个值(反正不能设为0,在卡尔曼滤波状态更新中这个值会自动调整)
• $Z_k$：是观测值,也就是在这里我们用这个不太靠谱的温度传感器测量出来的温度值/速度值等。
• $R$：是测量噪声协方差。这个不太靠谱的传感器的靠谱程度,换句话说,这个值你可以自己看着办,估摸着大概取个值。反正比最大误差尽量小点儿。

简化之后来看卡尔曼增益 ：${K}_k=\frac{P_{\bar{k}}}{P_{\bar{k}}+R}$。公式的字面意思是说：

卡尔曼增益：

（预测误差）占（预测误差 + 观测误差）总和的比重。

换句话说：预测误差在总误差中占多大分量。简言之：预测误差占比。

然后更新方式为：

最优估计 = 预测值 + 卡尔曼增益 * （观测值 - 预测值）

$$\begin{gathered} {K}_k=\frac{P_{\bar{k}}}{P_{\bar{k}}+R} \\ {P}_k=(I-K_k)P_{\bar{k}}=\frac{R}{P_{\bar{k}}+R}{P}_{\bar{k}} \end{gathered}$$

最优估计误差 = （1 - 卡尔曼增益）* 预测误差 = 测量误差占比 * 预测误差

原作者写作幽默风趣：我还是直接引用吧。

可以看到,从54次后,卡尔曼滤波器的预测逐步稳定在了34.2度之间,用狗话说,就是收敛了,通过卡尔曼滤波器,我们最终得出房间里的实际温度应该就是34.2度左右了，温度误差不过0.2度。

你可能马上一拍大腿又跳出来，麻蛋，我又被作者忽悠了，搞了那么多，不就是把全部测量值加起来然后取个平均么。

我言之：（其实是置信度加权平均。误差项交叉（协方差）相乘。）

如果你意识到了这一点，那么恭喜你你觉悟了，以后必定是我大信号系的一块好料，但

卡尔曼滤波是一个根据未来输入不断变换并自我调整的过程，

这点你取平均恐怕是办不到。你可能已经发现，

卡尔曼滤波本质上就是“更相信那些出现的比较多的，更不相信出现的比较少的”。

一个并不复杂的原理过程但需要统计学里的一些知识去解释，比如说32.5度这个测量值出现了5次，31.3度只出现一次，那么当然是更相信32.5往他那边靠啊，但数学表述需要一个严谨的、可量化的、范围化的过程，因此你最终看起来显得不那么好理解，究其本质实际上并没有公式上写的那么复杂。

---

---

面试答法：

朋友去其他公司面试算法类岗位的时候，问了她“卡尔曼滤波是什么鬼”还有“卡尔曼滤波的本质是啥”。

官腔点的：*卡尔曼滤波即用系统的观测去最优估计系统状态的方法。*这跟HMM的解码太一致了，即在模型参数已知的情况下，根据观测序列求最似然状态序列。HMM里的模型参数对应Kalman滤波里的状态方程与观测方程。

既然跟HMM随机过程有共通的地方，那么其本质也不难用随机方法来解答。可以这么说，卡尔曼滤波其本质是一个不断利用新信息去修正后验分布从而逼近最优分布的过程，初始的状态分布代表先验分布。求最优分布的的过程。

我觉得还是百度的那个回答更好听一些。

但我个人理解：更重要的是求：测量误差和预测误差。这个误差分布越符合正态分布，则预测越准。

卡尔曼滤波是一种统计意义上的数学优化方法。

既然是统计意义上的：所以大数定理效果明显非常，所以**统计量越大越准确，**量越小越偏离。

百度百科上说的这张图也是在说，这个越测越稳。

博文二中，作者也是在50多次之后，才说测量稳定的。

---

---

补充一个新的理解

卡尔曼增益的两个作用：

1、将观察值和预测值的残差从观察域映射到状态域

2、承担调节因子的作用：在运动方程给出的预测值和观测方程给出的观察值之间取折中。

噪声协方差（不确定性）如何更新

参考链接6. 第06课 视觉定位_哔哩哔哩_bilibili

在预测阶段中，需要对下一时刻的协方差进行递推预估，然后在更新阶段中，需要下一时刻的协方差进行更新。

实际代码OpenCV中有关于卡尔曼的实例，其相应的编程实现如下。

在视频中，讲解了状态转移矩阵和观测矩阵如何。

卡尔曼滤波器代码实例

可参考文献opencv实现卡尔曼滤波_opencv 卡尔曼滤波-CSDN博客

class CV_EXPORTS_W KalmanFilter
{
public:
    CV_WRAP KalmanFilter();
 
    //dynamParams状态的维度;measureParams 测量值的维度；controlParams控制向量的维数。
    CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
 
    //重新初始化卡尔曼滤波器
    void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
 
    //计算预测的状态,返回 statePre
    CV_WRAP const Mat& predict( const Mat& control = Mat() );
 
    //根据测量(观测)结果更新预测状态，返回statePost
    //measurement不能通过观测方程进行计算得到，要自己定义
    CV_WRAP const Mat& correct( const Mat& measurement );
 
    /*
       x(k) = A*x(k-1) + B*u(k)+w(k)         运动方程（w(k)协方差为Q）
       z(k) = H*x(k)   + v(k)                观测方程 (v(k)协方差为R)
    */
    CV_PROP_RW Mat statePre;           //预测值  (x'(k)): x(k)=A*x(k-1)+B*u(k)
    CV_PROP_RW Mat statePost;          //状态值  (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    CV_PROP_RW Mat transitionMatrix;   //状态转移矩阵 (A)
    CV_PROP_RW Mat controlMatrix;      //控制矩阵 B  
    CV_PROP_RW Mat measurementMatrix;  //测量矩阵 H
    CV_PROP_RW Mat processNoiseCov;    //系统误差 Q
    CV_PROP_RW Mat measurementNoiseCov;//测量误差 R
    CV_PROP_RW Mat errorCovPre;        //先验协方差(P'(k)): P'(k)=A*P(k-1)*A^T + Q)
    CV_PROP_RW Mat gain;               //卡尔曼增益 K(k)=P'(k)*H^T*inv(H*P'(k)*H^T+R)
    CV_PROP_RW Mat errorCovPost;       //后验协方差 (P(k)): P(k)=(I-K(k)*H)*P'(k)
 
    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

---

​ 翔【自动驾驶与机器人中的SLAM技术】学习笔记（八）卡尔曼滤波器四：一文理清卡尔曼滤波，从传感器数据融合开始谈起【转载】

获取Markdown格式

逍遥郎wj于 2024-08-12 22:11:49 发布

阅读量948 收藏 25

点赞数 13

分类专栏： 高翔书《自动驾驶与机器人中的SLAM技术》学习笔记 文章标签： 自动驾驶 机器人 学习

版权

高翔书《自动驾驶与机器人中的SLAM技术》学习笔记专栏收录该内容

15 篇文章17 订阅

订阅专栏

卡尔曼滤波器三：熟悉

引用来源：一文理清卡尔曼滤波，从传感器数据融合开始谈起

1 从传感器的测量谈起

在正式讨论卡尔曼滤波前，我们先讨论对物理量的测量。我们会发现是和卡尔曼滤波紧密相关的。 我们知道，如果需要对自然界的某个物理量，比如温度，气压，速度等进行测量，我们需要用各种传感器进行测量。但是，因为器件的工艺不可能达到完美，或者其他不能被人为预测到或者控制到的因素和噪声等存在，

传感器对物理量的预测不可能是完全准确的。因此，我们与其把传感器的测量结果当成是一个确定值，不如把它看成是一个随机变量 v ，

其均值和方差分别为μ , σ 2 \mu, \sigma^2μ,σ2，既是v ∼ P ( μ , σ 2 ) v\sim P(\mu, \sigma^2)v∼P(μ,σ2)，，这两个统计参数描述了测量的输出值（也就是我们直接观察到的值）和对这个测量的可信程度。**同时，我们要注意到，这里的*μ , σ 2 \mu, \sigma^2*μ*,*σ*2*不一定是时间平稳的，也就是说可能随着时间的变化而变化。 （暂且假设传感器的测量均值是和真实值无偏的。）

如下图所示，如果直接观察传感器数据，那么其可能是会存在很大的抖动，而不是平滑的，原因可能是观察噪声的影响。

Fig 1. 传感器的数据直接观测结果。

我们这个时候就想到，如果一次观察是抖动的，有着σ 2 \sigma^2σ2的不确定的，那么如果用同一个传感器对这个物理量观察N次，然后对N次数据进行求和，以减少不确定性的影响，岂不妙哉？ 这样的确是可以的，这个就是信号处理当中的滑动窗口均值滤波(mean filter)。但这个简单操作有几个缺点：

1. 我们前面谈到了不确定度σ 2 \sigma^2σ2是可能时变的，简单相加不能最好地消除不确定性。
2. 时间上滑动窗口进行多次测量的求和，会导致延迟。

对此，我们进行一个小改进，就是用多个相同的传感器去同时测量一个物理量，然后求和或者根据可靠程度去求加权平均和，我们假设多个传感器的采样值x i x_ix**i，满足分布，其中i表示传感器序号：
x i ∼ P ( μ , σ 2 ) (1) x_{i}\sim P(\mu,\sigma^2) \tag{1}x**i​∼P(μ,σ2)(1)
我们发现，其因为假设是无偏测量传感器因此均值相同，但是每个传感器的不确定性不一定相同。

这个时候简单的求和就容易造成结果的偏移，我们不妨根据方差的大小，进行加权平均求和，在此之前，我们需要几个假设：

1. 不同传感器的测量都是一个随机变量，其均值μ \muμ相同。
2. 不同传感器的测量之间是无关的，也就是说知道了x i x_ix**i不能对知道其他策略x j x_jx**j提供任何信息，但是也不会影响到观测x i x_ix**i的均值，即是E ( x j ∣ x i ) = E ( x j ) E(x_j\mid x_i) = E(x_j)E(x**j∣x**i)=E(x**j)

接下来，我们用这两个假设，进行简单的传感器间的数据融合以提高测量效果。Let’s move on！

2 简单版本，多传感器数据融合

为了简单起见，假设我们用两个相同的传感器进行测量，那么最后数据融合结果应该是：
x ^ = α x 1 + β x 2 α + β = 1 ⇒ x ^ = α x 1 + ( 1 − α ) x 2 (2) \hat{x}=\alpha x_1+\beta x_2\\alpha+\beta=1\\Rightarrow\hat{x}=\alpha x_1+(1-\alpha)x_2 \tag{2}x^{=*α**x*1​+*β**x*2​*α*+*β*=1⇒*x*}=α**x1​+(1−α)x2​(2)
那么，融合后的估计x ^ \hat{x}x^​的不确定度可以通过x 1 , x 2 x_1, x_2x1​,x2​的方差进行衡量，公式如:
σ ^ 2 ( α ) = ( 1 − α ) 2 σ 1 2 + α σ 2 2 (3) \hat{\sigma}^{2(\alpha)=(1-\alpha)}2\sigma_1^{2+\alpha\sigma_2}2 \tag{3}σ^2(α)=(1−α)2σ12​+α**σ22​(3)
为了最小化 σ ^ 2 ( α ) \hat{\sigma}^{2(\alpha)*σ*}2(α)，，我们用求导并且置为0的方法[3]，不难推导出当α = σ 1 2 σ 1 2 + σ 2 2 \alpha = \frac{\sigma_1^2}{\sigma_12 + \sigma_2^2}α=σ12​+σ22​σ12​​时，式子(3)有最小值，此时，式子(2)可化为:
x ^ ( x 1 , x 2 ) = σ 2 2 σ 1 2 + σ 2 2 x 1 + σ 1 2 σ 1 2 + σ 2 2 x 2 (4) \hat{x}(x_1,x_2)=\frac{\sigma_2^2}{\sigma_12+\sigma_2^{2}x_1+\frac{\sigma_1}2}{\sigma_1^2+\sigma_22}x_2 \tag{4}x^(x1​,x2​)=σ12​+σ22​σ22​​x1​+σ12​+σ22​σ12​​x2​(4)
这里讨论的只是两个传感器的情况，可以简单地推导到多个传感器的情况和当观测值是一个向量时候的情况，以及为了计算有效性，采用迭代计算的方法，具体可以参考文献[3]。其中，为了以后讨论的方便，这里给出当观测值是一个向量，并且只有两个传感器时的公式(5)：
x 1 ∼ p 1 ( μ 1 , Σ 1 ) , x 2 ∼ p 2 ( μ 2 , Σ 2 ) K = Σ 1 Σ 1 + Σ 2 x ^ = x 1 + K ( x 2 − x 1 ) Σ y y = ( I − K ) Σ 1 (5) \tag{5}​x1​∼p1​(μ1​,Σ1​),x2​∼p2​(μ2​,Σ2​)K=Σ1​+Σ2​Σ1​​x^=x1​+K(x2​−x1​)Σyy​=(I−K)Σ1​​(5)
可以发现，此时不再假设每个传感器的测量均值都是一样的了，其中的K称之为卡尔曼增益(Kalman Gain)，嘛，这里不过只是个名字，暂且不管。

3 卡尔曼滤波，开始征程

接下来我们开始正式讨论卡尔曼滤波(Kalman Filter)。我们之前讨论的传感器之间其实都是**无关(uncorrelated)**的，但是，其实经常我们知道了某个测量量，是可以确定或者为确定另一个测量量提供信息量的，比如我们现在需要测量车辆的位置和速度，那么知道了速度，通常可以为下一步知道位置提供一定的信息。在这种前提下，我们便能够通过更为合理的数据融合手段，得到更为精确的估计结果。

3.1 定位问题[1]

考虑一个例子，我们的机器人需要定位，通常使用的是GPS进行定位，得到车辆的状态量之一的位置：p。其次，我们可以通过测量轮子的转过的圈数，对机器人的运行速度进行测量，得到状态量另一个，速度：v。但是，我们要牢记，我们的观测不是完全准确的，比如GPS存在误差，而测量轮子转过的圈数也不能完美的描述速度，因为可能因为地面不平，轮胎打滑等原因导致误差。不过我们记住我们这个例子中的两个状态量：
x ⃗ k = { p ⃗ , v ⃗ } (6) \vec{x}_k={\vec{p},\vec{v}} \tag{6}x**k​={p​,v}(6)
在这个情况下，我们对两个状态量的观测其实是一个两元的概率变量，我们的每个观测都落在分布之中，而我们的任务就是从这个不确定性高的分布中，得到个不确定性更小的分布，从而得到更为精确的估计。图如：

Fig 2. 当两个状态量的观测存在相关性的时候，其观测可能的落在的分布图。

Fig 3. 当两个状态量的观测不存在相关性的时候，其观测可能的落在的分布图，其为一个水平于横轴的类似矩阵的区域。

这个时候，观测的不确定性体现在方差σ 2 \sigma^2σ2上，而观测值可以用均值向量μ \muμ描述，如Fig 4所示：

Fig 4. 观测中的 值和不确定性的表述。

3.2 状态预测方程

这个时候，如果我们的观测是准确的，那么会出现什么情况呢？假设为匀速运动。我们根据牛顿力学，可以对位置-速度的过程进行建模，我们会有：
p ⃗ k = p ⃗ k − 1 + Δ t ⋅ v ⃗ v ⃗ k = v ⃗ k − 1 (7) \tag{7}​p​k​=p​k−1​+Δt⋅vvk​=v**k−1​​(7)
用矩阵形式表达就是：
KaTeX parse error: Expected ‘EOF’, got ‘&’ at position 22: …\mathbf{x}}_{k}&̲ =\begin{bmatri…
我们称F k = [ 1 Δ t 0 1 ] \mathbf{F}_k=Fk​=[10​Δt1​]为预测矩阵(Predict Matrix)，通过此，我们可以用当前时刻k-1的状态量去预测下一个时刻k的状态量，即使我们并不知道真实的值应该是多少，但是这并不影响我们对状态的预测。

Fig 5. 通过建模，对下个时刻状态的预测。

当在用F k \mathbf{F}_kFk进行描述这个预测时，其实可以看成是点对之间的线性变换，那么如Fig 6所示：

Fig 6. 在线性变换下，点对的坐标变化情况。

那么此时，预测的状态量x ^ k \mathbf{\hat{x}}_{k}x**^k有了，还需要预测k时刻的不确定性**，也就是方差，在多变量情况下是协方差矩阵，用P k \mathbf{P}kPk表示:
P k = F k P k − 1 F k T (9) \mathbf{P}{k} =\mathbf{F}k\mathbf{P}{k-1}\mathbf{F}_k^T \tag{9}Pk​=Fk​Pk−1​FkT*​(9)
于是我们有，状态量预测和协方差预测：
x ^ k = F k x ^ k − 1 P k = F k P k − 1 F k T (10) \tag{10}​x****^{***k*​=**F***k*​**x****}k−1​Pk​=Fk​Pk−1​Fk**T*​​(10)

3.3 考虑施加外力的情况，添加控制项

在控制理论问题中，我们怎么能忘记添加控制项呢？毕竟我们都希望整个系统是可控制的，而不是任其随意发展的。

让我们继续扩展我们上面的那个例子。考虑到机器人本身有油门，可以进行一定的加速行驶，也可以按照一定的加速度制动，让我们假设这个加速度为a

那么根据牛顿力学，我们的状态预测方程(7)就更新为:
p k = p k − 1 + 1 2 a Δ t 2 v k = v k − 1 + a Δ t (11) \tag{11}​p**k​=p**k−1​+21​aΔt2v**k​=v**k−1​+aΔt​(11)
同样还是用矩阵形式表达，有:
x ^ k = F k x ^ k − 1 + [ Δ t 2 2 Δ t ] a = F k x ^ k − 1 + B k u k (12) \tag{12}x**^{***k*​​=**F***k*​**x****}k−1​+[2Δt2​Δt​]a=Fk​x****^k−1​+Bk​uk​​(12)
其中，B k \mathbf{B}_kBk​被称之为控制矩阵(Control Matrix)， u k \mathbf{u}_kuk​被称之为控制向量(Control Vector)** 。如果一个系统实在是没有控制项，那么可以忽视这个控制项。

然而，因为一系列的误差存在，我们的预测不可能是完全准确的。那么我们的误差或者是不确定性主要在哪里存在呢?

3.4 导致预测或者观察不确定性的因素

有以下四种因素可能导致我们的状态预测或者观察存在不确定性[2]，我们对此进行简单描述：

1. 参数的不确定性：参数不确定性指的是在对预测进行建模时，比如𝐹=𝑀𝑎，这个模型通过参数𝑀进行建模，然而对这个参数的观察不可能是百分百精确的，这个参数的误差就会导致整个模型的误差，因此状态预测这个时候更新为:

x ^ k = ( F k + Δ F k ) x ^ k − 1 + ( B k + Δ B k ) u k (13) \mathbf{\hat{x}}{k}=(\mathbf{F}{k}+\Delta\mathbf{F}{k})\mathbf{\hat{x}}{k-1}+(\mathbf{B}{k}+\Delta\mathbf{B}{k})\mathbf{u}_{k} \tag{13}x**^{***k*=(**F***k*+Δ**F***k*)**x****}k−1+(Bk+ΔBk)uk(13)

1. 控制器的不确定性：实际生活中，我们的控制器同样不可能完美，这个误差可以建模为:

x ^ k = F k x ^ k − 1 + ( B k + Δ B k ) u k (14) \mathbf{\hat{x}}{k}=\mathbf{F}{k}\mathbf{\hat{x}}{k-1}+(\mathbf{B}{k}+\Delta\mathbf{B}{k})\mathbf{u}{k} \tag{14}x**^{***k*=**F***k***x****}k−1+(Bk+ΔBk)uk(14)

1. 模型的不确定性：实际中，我们通过简单的线性建模不一定能很好地表达模型预测，因此要引入一个残差项，表示模型的不完美，建模为：

x ^ k = F k x ^ k − 1 + B k u k + f ( x ^ k − 1 , u k ) (15) \mathbf{\hat{x}}{k}=\mathbf{F}{k}\mathbf{\hat{x}}{k-1}+\mathbf{B}{k}\mathbf{u}{k}+f(\mathbf{\hat{x}}{k-1},\mathbf{u}_{k}) \tag{15}x**^{***k*=**F***k***x****}k−1+Bkuk+f(x**^k−1,uk)(15)

1. 观测不确定性：就像我们之前谈到的，我们的观测也是不完美的。

3.5 描述不确定性

正如上一节我们谈到的，有一些影响状态的因素，比如风，地面情况，轮胎打滑或者其他各种小情况我们是没法完全考虑到的，也就没法建模出来，这个时候，状态预测结果就存在不确定性，如Fig 7所示：

Fig 7. 预测的不确定性。

我们为了对未能跟踪到的变量进行统一建模，我们假设状态从x ^ k − 1 \mathbf{\hat{x}}{k-1}x**^**k−1到下一个时刻状态，其下一个状态落在一个协方差为Q k \mathbf{Q}{k}Qk的高斯分布中，也就是说，我们把所有未能跟踪到的影响因素都用这个高斯分布描述了。

这个影响导致了我们式子(10)中的协方差发生了变化，但是其均值不变，公式如:
x ^ k = F k x ^ k − 1 + B k u k P k = F k P k F k T + Q k (16) \tag{16}x**^{***k*​**P***k*​​=**F***k*​**x****}k−1​+Bk​uk​=Fk​Pk​Fk*T​+Qk*​​(16)
以上的内容只是对预测结果进行了讨论，但是实际上我们除了预测，还会存在传感器的测量，虽然这个测量是不准确的，但是也能提供一定的信息量。

3.6 根据观测结果对估计进行调整

预测和观测~同父异母的兄弟

实际系统中，我们可能有多个传感器给予我们关于系统状态的信息，我们这里不在乎其测量的量到底是什么，我们只要知道，每个传感器都间接地告诉了我们状态量。注意到我们预测状态量的尺度和单位和观测结果的尺度和单位可能是不一样的，这个时候就需要用线性变换把他们变成一样尺度和单位的。你可能猜到了，我们还是引入一个矩阵$$描述这个线性变换。

Fig 8. 预测和观察的尺度和单位不一致性。

Fig 9. 通过线性变换将预测的尺度和单位转换到和观测一致。

那么，公式有:
μ ⃗ e x p e c t e d = H k x ^ k Σ e x p e c t e d = H k P k H k T (17) \tag{17}μ​expected​Σexpected​​=Hk​x****^k​=Hk​Pk​HkT​​(17)
老问题，因为传感器存在噪声，我们的观测结果至少在某种程度上是不可靠的，在原来估计的情况下的结果可能对应了一个范围的传感器观测值。

Fig 10. 同一个预测可能对应多个观测。

为了描述这个不确定性（也就是传感器噪声），我们引入了R k \mathbf{R}{k}Rk，这个分布的均值和我们的观测值z k \mathbf{z}{k}zk相同，但是存在有不确定性，用协方差进行描述。

到现在为止，我们有了两个高斯分布：

1. 一个是通过线性变换H k \mathbf{H}{k}Hk将预测结果转化为理论观察值的分布H k x ^ k \mathbf{H}{k}\mathbf{\hat{x}}_{k}Hkx**^**k。
2. 另一个是实际的观察值的分布R k \mathbf{R}{k}Rk，其均值为z k \mathbf{z}{k}zk。

Fig 11. 两个描述同一个量的不同分布的交集。

不难发现，因为这两个分布都是描述同一个量，其交集处，如Fig 12所示，应该是最好的估计结果，我们看到这个交集处，其协方差明显比观测和预测的都要小得多，结果也就更为精确。

为了得到这个交集分布的表达形式，我们需要将两个高斯分布进行相乘即可，我们知道高斯分布的乘积也是高斯分布[4]。

Fig 12. 最佳估计其实也是符合高斯分布。

因此问题也就变成怎么求两个高斯分布的乘积的高斯分布的参数，如Fig 13所示：

Fig 13. 求蓝色分布的参数，高斯分布的乘积也是高斯分布。

根据[1,4]的推导，我们知道相乘后的结果为:
μ ′ = μ 1 + σ 1 2 ( μ 2 − μ 1 ) σ 1 2 + σ 2 2 ( σ ′ ) 2 = σ 1 2 − σ 1 4 σ 1 2 + σ 2 2 (18) \mu^{{\prime}=\mu_1+\frac{\sigma_1}2(\mu_2-\mu_1)}{\sigma_1^2+\sigma_22}\(\sigma^{\prime})2=\sigma_1^{2-\frac{\sigma_1}4}{\sigma_1^2+\sigma_22} \tag{18}μ′=μ1​+σ12​+σ22​σ12​(μ2​−μ1​)​(σ′)2=σ12​−σ12​+σ22​σ14​​(18)
通过引入K，可以进行一些化简有：
K = σ 1 2 σ 1 2 + σ 2 2 ⇒ μ ′ = μ 1 + K ( μ 2 − μ 1 ) ( σ ′ ) 2 = σ 1 2 − K σ 1 2 (19) \tag{19}K⇒μ′(σ′)2​=σ12​+σ22​σ12​​=μ1​+K(μ2​−μ1​)=σ12​−Kσ12​​(19)
如果用矩阵形式表达，则有：
K = Σ 1 Σ 1 + Σ 2 μ ′ → = μ 1 → + K ( μ 2 → − μ 1 → ) ∑ ′ → = Σ 1 − K Σ 1 (20) \tag{20}​K=Σ1​+Σ2​Σ1​​μ′​=μ1​​+K(μ2​​−μ1​​)∑′​​=Σ1​−KΣ1​​(20)
其中Σ 1 , Σ 2 \Sigma_1, \Sigma_2Σ1​,Σ2​分别是预测和观测的协方差矩阵。其中如(5)，我们把K称为卡尔曼增益，我们发现(20)和(5)形式上是一致的。

4 结合起来吧~状态更新

我们现在有两个分布了：

1. 预测分布:

μ 1 → = H k x ^ k Σ 1 = H k P k H k T (21) \overrightarrow{\mu_1}=\mathbf{H}_k\mathbf{\hat{x}}_k \ \Sigma_1=\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T \tag{21}μ1=Hkx**^kΣ1=HkPkHk*T*(21)

1. 观测分布:

μ 1 → = z k Σ 1 = R k (22) \overrightarrow{\mu_1}=\mathbf{z}_k \ \Sigma_1=\mathbf{R}_k \tag{22}μ1=zkΣ1=Rk(22)

将(21) (22)代入(20)，我们有:
H k x ^ ′ k = H k x ^ + K ( z k − H k x ^ ) H k P ′ k H k T = H k P k H k T − K H k P k H k T (23) \tag{23}Hk​x****^{**′*k*​**H***k*​**P**′*k*​**H***k**T*​​=**H***k*​**x****}+K(zk​−Hk​x****^)=Hk​Pk​HkT*​−KHk​Pk​HkT​​(23)
注意到(23)的H k \mathbf{H}_{k}Hk​可以进行约减，最后得到：
x ′ ^ k = x ^ k + K ′ ( z k − H k x ^ k ) P ′ k = P k − K ′ P k (24) \tag{24}x′^{*k*​**P**′*k*​=**x****}k​+K′(zk​−Hk​**x***^k​)=Pk​−K′Pk​​(24)
其中，卡尔曼增益变为:
K ′ = P k H k T H k P k H k T + R k \text{K}^{{\prime}=\frac{\mathbf{P}_k\mathbf{H}_k}T}{\mathbf{H}_k\mathbf{P}_k\mathbf{H}_k^T+\mathbf{R}_k}K′=Hk​Pk​HkT​+Rk*​Pk​HkT*​​
这样，我们就得到了最后的结合了观测和模型预测的最佳估计x ′ ^ k \hat{\mathbf{x}^{{\prime}}_{k}**x**′}k​了，并且知道了P ′ k \mathbf{P}^{\prime}kP′k，可以为下一步的迭代更新提供先验了。我们发现，其实观测为预测提供了先验知识。这个过程可以一直迭代更新。整个卡尔曼滤波的流程如Fig 14所示。

Fig 14. 卡尔曼滤波的整个流程。

Reference

[1]. https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

[2]. https://towardsdatascience.com/kalman-filter-intuition-and-discrete-case-derivation-2188f789ec3a

[3]. Pei Y, Biswas S, Fussell D S, et al. An elementary introduction to kalman filtering[J]. arXiv preprint arXiv:1710.04055, 2017.