卡尔曼滤波器的特殊案例

点击上方“3D视觉工坊”,选择“星标”
干货第一时间送达

什么是卡尔曼滤波器?

卡阿尔曼滤波器为每个结果状态找到最佳的平均因子。另外,以某种方式保存过去的状态。它针对每个时间范围对变量执行联合概率分布。该算法对每个步骤使用新的均值和新方差,以便计算结果的不确定性,并尝试为测量更新(传感/预测)和运动更新(运动)的每个时间范围提供准确的测量。该算法还使用其他误差和统计噪声来表示初始的不确定性。

卡尔曼滤波器的目的

• 将来自各种传感器(如LiDAR和Radar跟踪器)的数据输入转换为可用形式。计算和推断速度。

• 减少目标位置和速度的测量误差(噪声)。

• 使用先前的状态估计和新数据预测目标的未来状态。

• 简单,实用和可移植的算法。

• 估计一个连续状态和结果,卡尔曼滤波器给了我们一个单峰分布。

卡尔曼滤波器的工作

卡尔曼过滤为我们提供了一种数学方法,这种方法依据物体的初始位置和相关变量来推断物体之后的运动速度和状态。因此,在这里,我们将创建一个一维卡尔曼滤波器,设置初始位置,结合物体运动的不确定性,来估计物体未来的位置以及运动速度。此外,如果我们想了解卡尔曼滤波器的工作原理,我们首先需要了解一些有关高斯的知识,它代表卡尔曼滤波器中的单峰分布。

高斯是在位置空间上的连续函数,其下面的面积之和最多为1。高斯的特征在于两个参数,平均值,经常缩写为希腊字母μ,高斯的宽度通常被称为方差即σ ²。所以,我们常用平均值和方差来寻找对象的位置,获得最佳估计。另外,宽度越大,不确定性越大。

一维高斯方程

上图表示高斯的均值(μ)和方差(σ²)。平均值(μ)越高,物体在该位置出现的机会就越高。相反,如果方差(σ²)较大,即分布较宽,则该对象的不确定性较高;可以放置在高斯内部的任何位置。

就公式而言,它是二次函数的指数,我们取表达式的指数。我们的查询点x相对于平均值(μ)的二次方差,除以方差(σ²),乘以-(1/2)。现在,如果x =μ,则分子变为0,如果x为0,即1。事实证明,我们必须在2πσ²的平方根上对常数进行归一化处理。

高斯特性

高斯是其特征在于通过给定的指数函数平均值(μ),其限定了高斯曲线的峰值的位置,和一个方差(σ ²)限定曲线的宽度/扩散。所有高斯都是:对称的。它们具有一个峰(也称为“单峰”分布),并且在该峰的两侧均具有指数下降。

方差是高斯分布的度量;方差越大,对应的高斯越短。差异也是确定性的度量;如果试图找到最确定的位置,例如汽车的位置,则需要一个高斯函数,其均值是汽车的位置并且不确定性/传播范围最小。我们编写一个高斯函数:

from math import *import matplotlib.pyplot as pltimport numpy as np# gaussian functiondef f(mu, sigma2, x):    ''' f takes in a mean and squared variance, and an input x       and returns the gaussian value.'''    coefficient = 1.0 / sqrt(2.0 * pi *sigma2)    exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)    return coefficient * exponential

改变均值

在卡尔曼滤波器中,我们迭代使用贝叶斯规则的测量(测量更新),贝叶斯规则仅是乘积或乘法,而运动更新(预测)则使用总概率(卷积或加法)进行迭代。

为了理解周期,我们假设我们正在对车辆进行定位并且我们有一个先验分布(蓝色高斯);这是一个非常宽泛的高斯平均值。现在,我们得到一个测量值(橙色高斯),它告诉我们有关车辆定位的信息。这是我们之前的一个例子,我们对位置不确定,但测量结果告诉我们有关车辆的位置信息。

事先分配和测量分配

注意:在上图中,Mu(μ)是先前的均值,Nu(v) 是新的测量均值。最终均值在两个旧均值,先验均值和测量均值之间移动。在测量方面,它要稍远一些,因为与以前相比,该测量可以更确定地确定车辆的位置。我们越确定,就越会在确定答案的方向上拉均值。

新高峰在哪里?

所得的高斯比两个分量的高斯更确定,即协方差小于设备中两个协方差中的任一个。直观上来说,是因为我们实际上获得了位置信息。在任一高斯装置中,这两个高斯都具有较高的信息量。

测量更新的公式

1.假设我们按照贝叶斯规则将两个高斯乘以一个先验概率和一个测量概率。先验的均值为Mu (μ),且为σ²,方差为Nu (v),协方差为r-square (r²)。

2. 然后,新的均值Mu prime (μ')是旧均值的加权和。Mu (μ)由r平方(r²)加权,Nu (v)由Sigma平方(σ²)加权,并由加权因子之和标准化。新的方差项将是Sigma平方素数(σ²')。

3. 显然,先验的高斯不确定性要高得多,因此σ²更大,这意味着Nu(v)的权重比Mu (μ)大得多。需要注意的是,方差项不受实际方法的影响,它仅使用以前的方差。

def update(mean1, var1, mean2, var2):''' This function takes in two means and two squared variance terms,       and returns updated gaussian parameters.'''#Calculate the new parameters   new_mean = (var2*mean1 + var1*mean2)/(var2+var1)   new_var = 1/(1/var2 + 1/var1)

高斯运动:

运动更新的公式

新的均值(μ')是旧均值Mu (μ)加上u的运动。因此,如果在x方向上移动了10米,那么它就是10米,并且知道σ²'是旧σ²加上运动高斯的方差(r²)。这就是我们所需要知道的,仅仅是补充。在预测步骤中得到的高斯只是将这两件事加起来,即mu(μ)加u和σ²加方差(r²)。

def predict(mean1, var1, mean2, var2):''' This function takes in two means and two squared variance terms, and returns updated gaussian parameters, after motion.''' #Calculate the new parameters new_mean = mean1 + mean2 new_var = var1 + var2 return [new_mean, new_var]

过滤器管道:

# measurements for mu and motions, Umeasurements = [5., 6., 7., 9., 10.]motions = [1., 1., 2., 1., 1.] # initial parametersmeasurement_sig = 4.motion_sig = 2.mu = 0.sig = 10000.  #0000000001 ## TODO: Loop through allmeasurements/motions## Print out and display the resultingGaussian# your code herefor i in range(len(measurements)):    #measurement update, with uncertainty   mu, sig = update(mu, sig, measurements[i], measurement_sig)   print('Update: [{}, {}]'.format(mu, sig))    #motion update, with uncertainty   mu, sig = predict(mu, sig, motions[i], motion_sig)   print('Predict: [{}, {}]'.format(mu, sig))   # print the final, resultant mu, sigprint('\n')print('Final result: [{}, {}]'.format(mu,sig))

•现在,我们将所有内容放在一起编写一个具有这两个功能的主程序,进行更新和预测,并馈入一系列测量和运动。选择的示例中,测量值= 5,6,7,9,10,运动是1,1,2,1,1。如果初始估算为5,但我们将其设置为0,不确定性为10,000。

•假设测量不确定度为常数4,运动不确定度为常数2。运行该变量时,位置的第一个估算值基本上应为5–4.99,原因是初始不确定性太大,因此估算值占主导地位通过第一次测量。不确定性减少到3.99,这比测量不确定性要好一些。然后,预测将增加1,但是不确定性增加到5.99,这是运动不确定性2。

•再次基于度量6更新,得到的估计值为5.99,几乎是6。再次移动1。测量了7。移动了2.测量了9.移动了1.测量了10,然后移动了最终1。结果作为最终结果,该位置的预测为10.99,即10位置移动了1 ,以及不确定性(残余不确定性)4。

绘制高斯:

## Print out and display the final,resulting Gaussian# set the parameters equal to the output ofthe Kalman filter resultmu = musigma2 = sig # define a range of x valuesx_axis = np.arange(-20, 20, 0.1)# create a corresponding list of gaussianvaluesg = []for x in x_axis:g.append(f(mu, sigma2, x))# plot the resultplt.plot(x_axis, g)

过滤器的实际工作方式

请记住,在测量更新中,measurement_sig(var2或σ²)= 4,在运动更新(预测)中,motion_sig(var2或σ²)= 2将保持恒定。除此之外,代码中的每个变量都会在每个时间步更新。例如,在过滤器管道的for循环中,mu和sig值将在测量更新中得到更新,然后将新的更新的mu和sig值输入到运动更新(预测)功能中。然后再次通过运动更新(预测)功能生成新值时,将这些更新后的值输入到测量更新(更新)功能中,循环继续进行,直到为车辆/物体的每个时间步长计算不确定性为止。

本文仅做学术分享,如有侵权,请联系删文。

下载1

在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

重磅!3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近3000星球成员为创造更好的AI世界共同进步,知识星球入口:

学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用,麻烦给个赞和在看~  

3D视觉工坊 CSDN认证博客专家 算法 3D视觉
个人公众号:3D视觉工坊。公众号特邀嘉宾及合伙人,先后就职于国内知名研究机构、自动驾驶公司、海康研究院,主要研究方向为深度学习、目标检测、语义分割、图像处理、自动驾驶感知算法等,CSDN博客专家。博主先后任职于国内知名研究院、知名大厂,致力于3D视觉算法、VLAM算法开发,涉及相机标定、手眼标定、结构光、点云后处理、三维重建等相关领域的研究,同时也是CSDN博客专家。3D视觉工坊坚持原创,近一年来输出了非常多的高质量文章,获得了粉丝的一致好评,我们将始终坚持走原创路线,打造一个铁杆粉丝的聚集区。
已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 书香水墨 设计师:CSDN官方博客 返回首页
实付 49.90元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值