Kalman filter到底是怎么工作的本文主要参考的文章https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/图片也基本来自上述博客其实接触KF已经很久了听过对应的课程也对着公式进行过推导但总有一种感觉始终在门外没有醍醐灌顶融会贯通的感觉所以还是写篇博客毕竟通过强行让自己输出一下也会帮助理解和记忆。一、什么是Kalman filter说到KF就LZ目前有限的接触中来说有SLAMtracking等一些领域。但是实际上卡尔曼滤波的应用远大于此甚至在卫星导航等领域也是用到这个技术那么这么牛的技术到底是什么呢假设存在一个动态系统这个系统中存在很多不确定的因素那么我们就可以通过使用卡尔曼滤波根据系统之前的一些数据后面会解释有哪些来进行预测预测这个系统下一步到底会做什么。如果用到卫星导航上也就是我们可以预测下一步卫星的位置大概率会在哪呢当然针对一个比较复杂的系统会存在很多不确定因素会存在噪声等但是按照原文和实际操作的工程经验来说似乎卡尔曼也能很好的解决此类问题呢。当然卡尔曼滤波也不是万能的它会有一定的假设前提比如说跟踪也许预测一帧到两帧是还不错的如果预测比较多的帧那预测不准也是情理之中了。那为什么工程上还是很喜欢用它呢因为在上述的应用场景中都是存在连续不断的变化卡尔曼滤波是非常使用这种场景的。此外它的内存占用很小只需要保留前一个状态速度非常快是实时问题和嵌入式系统的理想选择。所以在无人驾驶中也会经常使用卡尔曼滤波来做一些应用毕竟不能在车上放台3090吧哈哈LZ倒是想呢。。。二、我们可以用卡尔曼滤波干什么呢举个小例子假设我们有个小机器人头顶一个天线就叫它一毛吧在树林里四处游走一毛需要进行导航所以它得知道它到底在哪才行就像我们去景区每个路标指示牌都会指明我们当前位置在哪一个道理一毛也需要知道。我们可以假设一毛在k时刻的状态是x k ⟶ \stackrel{\longrightarrow}{x_k}xk​⟶​, 这个latex的写法是这样的\stackrel{\longrightarrow}{x_k}也就是一毛现在的状态x k ⟶ \stackrel{\longrightarrow}{x_k}xk​⟶​包含对应的位置信息和速度信息注意在这个例子中状态是位置和速度放进其他问题它可以是水箱里液体的提及汽车引擎的温度或者是任何其他的数据。例如在跟踪中就可能是classKalmanFilter(object): A simple Kalman filter for tracking bounding boxes in image space. The 8-dimensional state space x, y, a, h, vx, vy, va, vh contains the bounding box center position (x, y), aspect ratio a, height h, and their respective velocities. Object motion follows a constant velocity model. The bounding box location (x, y, a, h) is taken as direct observation of the state space (linear observation model). 包括的是bbox的中心位置宽高比a和高度h还有它们对应的速度。还是回到一毛这里假设我们的一毛有一个GPS定位装置定位的精度为10m可能在树林里定位容易不太准现在GPS的精度实际上应该可以高很多吧。但是10m的误差还是让一毛不太放心万一在树林里突然出现个悬崖峭壁我们一毛不久就地阵亡了嘛所以单纯的依赖GPS提供的信息还是不够好当然我们也可以预测一毛是怎么移动的它自己把指令发送给控制轮子的马达如果它始终朝着一个方向走并且当中没有遇到什么障碍那么下一个时刻很有可能还是朝着这个方向行驶。但是一毛对自己的状态也不是完全清楚的它有可能会轮子打滑或者上个坡再下个坡。。。那么如果出现这种情况一毛就不能通过轮子转动的次数来确定实际的距离了所以基于这个距离的预测也是不完美的举个实际的例子吧之前LZ玩小车的时候如果四个轮胎的胎压不同那么小车也是走不了直线的这种都是会存在误差的按照上述的情形我们可以知道GPS为一毛提供了一些关于状态的信息但是这个信息是间接的不准确的一毛自食其力根据自己之前的状态预测了自己的位置但那也是间接的存在不确定性的万一轮胎漏个气呢是吧。但是上述的信息就是我们能够获得的全部的信息了在这些信息的基础上我们怎么能利用这些信息给出一个完整的预测呢让我们预测的准确度比一毛用GPS或者自食其力预测的准确度更高呢此时就是卡尔曼滤波上场的时刻啦三、卡尔曼滤波是怎么看待一毛这个问题的呢我们继续上面一毛的问题假设有一个简单的状态只包含位置和速度实际上我们并不知道真实的位置和速度是什么所以位置和速度会有大量不同的组合其中某些组合的可能性会更加高一些如下图所示越亮的位置代表可能性越高。卡尔曼滤波假设它的变量都是随机的并且都是符合高斯分布的。在本文的这个例子中其实可以看作一毛的位置和速度其实都是随机的并且符合高斯分布的。每个变量都会有一个均值μ \muμ它是代表随机分布的中心其实就是它最有可能出现的状态还有一个是方差σ 2 \sigma^2σ2它是用来表示不确定性的画个比较夸张的图吧不确定性越大那么高斯分布的那个钟型越胖一些这个是可以理解的因为出现其他数值的可能变大了嘛看图加理解不需要死记硬背哦要是想自己画一下的小伙伴LZ这里也提供一下代码吧importnumpyasnpimportmatplotlib.pyplotaspltimportmathdefnormal_distribution(x,mean,sigma):returnnp.exp(-1*((x-mean)**2)/(2*(sigma**2)))/(math.sqrt(2*np.pi)*sigma)mean1,sigma10,0.5x1np.linspace(mean1-6*sigma1,mean16*sigma1,100)mean2,sigma20,2x2np.linspace(mean2-6*sigma2,mean26*sigma2,100)mean3,sigma35,0.5x3np.linspace(mean3-6*sigma3,mean36*sigma3,100)y1normal_distribution(x1,mean1,sigma1)y2normal_distribution(x2,mean2,sigma2)y3normal_distribution(x3,mean3,sigma3)plt.plot(x1,y1,r,labelm0,sig0.5)plt.plot(x2,y2,g,labelm0,sig2)plt.plot(x3,y3,b,labelm5,sig0.5)plt.legend()plt.grid()plt.savefig(./test_gaussian.jpg)plt.show()在下面这幅图中位置和速度是不相关的也就是说状态中一个变量的信息并不能决定另外一个变量。如果位置和速度是相关的观测到一个特定位置的可能性取决于其对应的速度这个情况应该不是很难理解如果一毛跑得飞快那么一毛移动的更远那么一毛距离上一个时刻的位置就会比较远啦但是如果一毛是刚吃完饭那种悠闲散步那可能速度很慢那么对应的位置移动也会比较小了。跟踪变量直接的潜在关系是非常重要的因为它能够给我们提供更多的信息通过一个观则可以告诉我们其他观测的可能信息。卡尔曼滤波的目的就是这个从不确定的观测中榨取尽可能多的信息这种变量之间的相关信息可以通过协方差矩阵covariance matrix来进行表示。简而言之在协方差矩阵中Σ i j \Sigma_{ij}Σij​就可以表示第i个状态变量和第j个状态变量之间的相关程度我们也可以从这个定义中推测出协方差矩阵是对称的也就是说我们沿着对角线进行翻转值是不会发生变化的。有一点需要说的是我们通常使用“Σ \mathbf{\Sigma}Σ”来表示协方差矩阵所以它的元素就可以用“Σ i j \Sigma_{ij}Σij​”来表示啦。四、使用矩阵来进行问题描述我们可以把从状态中得到的信息建模成一个高斯斑Gaussian blob所以在k时刻我们需要两个信息最佳估计X ^ k \hat{\mathbf{X}}_kX^k​(也就是我们刚刚分析高斯分布的均值上文中我们称为μ \muμ),还有它对应的协方差矩阵P k \mathbf{P_k}Pk​虽然在这个例子中我们还是只使用位置和速度这两个变量实际上我们可以使用任意数量的变量来进行数学建模。接下来我们需要某种方式来查看当前的状态time: k-1)并且对下一时刻time k的状态进行预测。值得注意的是我们不知道到底哪一个状态才是真实值因为我们预测函数并不关注这个问题它适用于所有的情况并且给出一个新的分布。我们可以用矩阵F k \mathbf{F_k}Fk​来表示这个预测的步骤从上图可知它把原始预测中的每个点都移到的新的预测的位置中如果原始预测是正确的话系统就会移到新的预测的位置。我们是如何使用矩阵来预测未来下一时刻的位置和速度的呢我们将使用一个非常基本的运动学公式写成矩阵的形式就是我们现在有一个可以给出下一个状态的预测矩阵但是我们仍然不知道怎么更新协方差矩阵因为上面预测的公式相当于只是更新了μ \muμ这个值这里我们就需要另外一个公式如果我们把分布中的每个点乘以了一个矩阵A \mathbf{A}A那么它对应的协方差矩阵Σ \mathbf{\Sigma}Σ会怎么变化呢恩这个会有以下的恒等式将等式4和等式3相结合可以得到下列等式五、外部影响实际上事情也并不总是一帆风顺的总会有一些外在的因素来影响到这个系统的状态例如模拟火车运动的时候除了列车自动驾驶之外列车操作员可能会手动调速。在一毛的例子中导航软件也可能发出停止的命令。对于上述的这些外界因素我们可以再引入一个向量u k ⟶ \mathbf{\stackrel{\longrightarrow}{u_k}}uk​⟶​作为对系统的修正来加入系统的预测中去。假设我们可以通过控制油门或者指令来产生一个加速度a由基本运动学我们可以知道写成对应矩阵的形式B ⟶ k \mathbf{\stackrel{\longrightarrow}{B}_k}B⟶​k​被称作是控制矩阵u ⟶ k \mathbf{\stackrel{\longrightarrow}{u}_k}u⟶k​被称作是控制向量如果存在一个非常简单的外部系统我们也可以省略掉对应的外部影响。我们还要考虑一个问题如果我们的预测并不是一个100%准备的模型那么又会发生什么呢六、外部的不确定性如果这个状态是基于自身的属性而发展那么事情发展会很好。但是如果状态是基于外力进行发展的那么事情的发展也不会超出预期只要我们能够知道这些外力是什么。但是如果我们不知道外力是什么呢举个例子如果我们在跟踪一个四轴飞行器它可能会受到风的冲击或者我们的一毛在冰上或者泥里打滑了我们就没有办法来跟踪到这些情况来。如果上述的情况发生了我们的预测就很有可能出错因为我们没有考虑到这个新的不确定性。通过在每个预测步骤后添加一些新的不确定性我们可以对与“世界”相关的不确定性进行建模即我们无法跟踪的事物如上图所示我们最初估计的每个状态可能移动到其他多个状态因为我们的假设都是高斯分布这次也不例外我们也可以说在状态x ^ k − 1 \hat{x}_{k-1}x^k−1​中每个点可能会移动到一个高斯斑中的某个位置并且符合方差为Q k \mathbf{Q}_kQk​换句话说我们可以将无法确定的外部因素描述成方差为Q k \mathbf{Q}_kQk​的高斯噪声。这会产生一个新的高斯斑有相同的均值但是方差不同。我们可以通过将Q k \mathbf{Q}_kQk​进行简单的叠加就可以得到完整的预测方程从式7中我们可以得出这个结论新的最佳观测是由上一个时间的最佳观测加上已知的外部修正得到的新的不确定性可以通过老的不确定性加上一些环境中的不确定性得到。到此我们得到了系统对应的一个模糊的估计并且由x ^ k \mathbf{\hat{x}}_kx^k​和P k \mathbf{P}_kPk​来确定对应的均值和方差。这些是一毛自食其力获得的数据那么如果我们还能获得一些传感器的数据呢七、通过测量值来改善估计我们也许还会有一些传感器能我们提供一些系统的信息目前我们也不关注测量值到底是什么对于一毛来说也许是GPS读到的位置或者有什么其他测速仪器的读数。每个数据都间接地告诉我们一些有关状态的信息传感器是在某个状态上运行并且产生一系列读数。值得注意的是传感器测量的读数的单位和尺度可能和我们跟踪状态的单位和尺度并不一致我们需要通过矩阵H k \mathbf{H}_kHk​来对传感器进行建模。我们可以用常见的方式来表达传感器读数的分布卡尔曼滤波的一个优点就是能够处理传感器噪声的问题。换句话说我们的传感器的数据多多少少会有些噪声不是绝对可靠的在我们最初估计的每个状态其实有可能存在一系列的传感器读数。从我们观察到的每个读数我们可以推测到系统应该会处于某个状态。但是由于我们的读数存在不确定性有些状态的可能性会比其他一些状态的可能性更高一些。我们把测量值例如传感器的噪声的不确定性的方差称作R k \mathbf{R}_kRk​我们观测到的读数同样也是服从高斯分布的假设这个分布的均值为z k ⟶ \stackrel{\longrightarrow}{\mathbf{z}_k}zk​⟶​所以我们就会有两个高斯斑一个是一毛自食其力根据自己的速度之类的推测出的一个状态还有一个是通过GPS给定的位置的一个状态也就有了这两个高斯斑换句话说我们会有两个预测值但是我们使用哪一个或者怎么把这两个预测的状态融合起来呢我们必须尝试把所有得到的信息都利用起来这样才能预测到一个更好的状态。我们来回顾一下具体有哪些信息呢以一毛为例我们可以通过代码设置或者轮子的转速和半径得到它的速度和位置并不是100%准确的同是还会出现外界因素影响上图中通过这个信息预测的状态用粉色进行表示一毛高大上的GPS定位装置可以直接读出它的位置也不是100%准确的上图中通过这个信息预测的状态用 绿色进行表示。所以我们将这个两类信息进行融合后我们又会得到怎样的状态呢对于任何可能的读数( z 1 , z 2 ) (z_1, z_2)(z1​,z2​)我们有两个相关的概率1传感器读数z k ⟶ \stackrel{\longrightarrow}{\mathbf{z}_k}zk​⟶​是读数z 1 , z 2 z_1, z_2z1​,z2​错误测量的概率2我们之前的估计认为( z 1 , z 2 ) (z_1, z_2)(z1​,z2​)是我们应该看到的读数的概率说点人话吧其实就是我们能得到两个状态k的估计( z 1 , z 2 ) (z_1, z_2)(z1​,z2​)也就是上面说的两个高斯斑但是它们也会出现一个问题就是哪个高斯斑更加符合系统实际的情况呢换句话说是传感器的读数更接近系统的真实状态呢还是说通过之前状态进行预测的值更接近系统的真实状态呢实际上如果我们有两个概率分布并且我们想知道两个都为真的概率我们只需要把他们相乘就可以得到对应的概率按照上图显示通过将两个高斯斑相乘我们可以得到两个斑点重叠的部分即两个斑点都很亮的部分概率比较大的位置可以看到中间重叠的区域比我们之前估计的范围要小很多所以估计会比之前的估计精确很多。这个分布的均值是两种估计都最有可能出现的设置因此在知道所有信息的情况下它是对状态最佳的猜测。恩从下图看出新产生的预测又像另外一个高斯斑实际证明如果将两个高斯斑相乘两个高斯斑对应各自的均值和方差我们就可以得到一个新的高斯斑新的高斯斑的均值和方差可以从原来的两个高斯斑的参数推导得到。八、高斯乘法我们从一维的数据开始一个一维高斯曲线均值为μ \muμ 方差为σ 2 \sigma^2σ2可以写成如下的表达式我们想知道如果两个高斯曲线相乘那么会发生什么状态呢下图中蓝色的曲线代表两个高斯曲线没有归一化的交集。我们把式9带入到式10中并做一些变化要注意进行归一化的问题来保证总的概率值为1可以得到如下我们可以提取一部分公有因子k kk来简化公式我们之前是怎么估计对应的预测值的呢仅仅是获取之前的估计加上一些新的项就可以得到新的估计上述公式还是非常简单的。上面举的例子是一维的例子如果是矩阵的版本呢我们来重新改写一下式12和式13。假设Σ \SigmaΣ表示高斯斑的协方差矩阵μ ⟶ \stackrel{\longrightarrow}{\mu}μ⟶​是代表它沿每个轴的均值矩阵K \mathbf{K}K我们就称做是卡尔曼增益后面我们会使用到它的加油小伙伴们胜利就在眼前啦九、综合所有的信息我们有两个分布通过预测数据满足下面这个分布通过观测数据满足下面这个分布我们按照式15将上面这两个分布相乘得到新的分布重叠区域的均值和方差其中从式14中我们可以得到对应的卡尔曼增益为我们将式16和式17中的公共项H k \mathbf{H}_kHk​从两边约去注意式17的K \mathbf{K}K中也有这个公共项H k \mathbf{H}_kHk​并且在P k ′ \mathbf{P}{_k}Pk′​中有H k T \mathbf{H}{^T_k}HkT​这个公共项约分简化后的表达式如下目前为止我们得到了每个状态的更新步骤x ~ k ′ \widetilde{x}{_k}xk′​是我们的最佳的预测值我们可以持续迭代连同P k ′ \mathbf{P}{_k}Pk′​输入到下一轮的预测或者更新中总的流程大图按照上面大图其实总的流程应该是这样的1我们先通过上一轮时刻的状态进行估计会得到对应的一个预测状态2然后我们会获得当前状态传感器的读数也会获得一个状态3然后我们将两个状态的估计进行融合更新得到最后的最佳状态分布十、总结在上面所有的数学公式中我们只需要实现的是方程718和19如果有兴趣的话也可以自己推导当中的过程。卡尔曼滤波使得我们可以准确对任意线性系统进行建模对于非线性系统我们需要使用扩展卡尔曼滤波EKF这个滤波器通过简单地线性化预测和测量的均值来进行后续的状态估计。十一、感想小伙伴看完一遍如果觉得云里雾里的这是很正常的毕竟卡尔曼滤波还是需要花费一定的时间理解的。如果第一遍没有理解不妨调整好心态再来一遍毕竟胖子也不是一口吃成的吧。