史上最详细的Kalman滤波解读与实践(python) |
您所在的位置:网站首页 › 滤波去噪的名词解释 › 史上最详细的Kalman滤波解读与实践(python) |
项目课题当中有使用到Kalman滤波的算法思想,这里总结一下这个神奇算法的过程。 什么是卡尔曼滤波?对于这个滤波器,我们几乎可以下这么一个定论:只要是存在不确定信息的动态系统,卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰,卡尔曼滤波通常也能很好的弄清楚究竟发生了什么,找出现象间不易察觉的相关性。 因此卡尔曼滤波非常适合不断变化的系统,它的优点还有内存占用较小(只需保留前一个状态)、速度快,是实时问题和嵌入式系统的理想选择。 如果你曾经Google过卡尔曼滤波的教程(如今有一点点改善),你会发现相关的算法教程非常可怕,而且也没具体说清楚是什么。事实上,卡尔曼滤波很简单,如果我们以正确的方式看它,理解是很水到渠成的事。 本文会用大量清晰、美观的图片和颜色来解释这个概念,读者只需具备概率论和矩阵的一般基础知识。 我们能用卡尔曼滤波做什么?让我们举个例子:你造了一个可以在树林里四处溜达的小机器人,为了让它实现导航,机器人需要知道自己所处的位置。 我们的小机器人装有GPS传感器,定位精度10米。虽然一般来说这点精度够用了,但我们希望它的定位误差能再小点,毕竟树林里到处都是土坑和陡坡,如果机器人稍稍偏了那么几米,它就有可能滚落山坡。所以GPS提供的信息还不够充分。 这个问题下,GPS为我们提供了一些关于状态的信息,但那是间接的、不准确的;我们的预测提供了关于机器人轨迹的信息,但那也是间接的、不准确的。 但以上就是我们能够获得的全部信息,在它们的基础上,我们是否能给出一个完整预测,让它的准确度比机器人搜集的单次预测汇总更高?用了卡尔曼滤波,这个问题可以迎刃而解。 卡尔曼滤波眼里的机器人问题还是上面这个问题,我们有一个状态,它和速度、位置有关: 那么如果位置和速度相关呢?如下图所示,机器人前往特定位置的可能性取决于它拥有的速度。 这种关系对目标跟踪来说非常重要,因为它提供了更多信息:一个可以衡量可能性的标准。这就是卡尔曼滤波的目标:从不确定信息中挤出尽可能多的信息! 为了捕获这种相关性,我们用的是协方差矩阵。简而言之,矩阵的每个值是第 i 个变量和第 j 个变量之间的相关程度(由于矩阵是对称的, i 和 j 的位置可以随便交换)。我们用 Σ 表示协方差矩阵,在这个例子中,就是
Σ
i
j
Σ_{ij}
Σij 为了把以上关于状态的信息建模为高斯分布(图中色块),我们还需要 k 时的两个信息:最佳估计
x
^
k
\hat{x}_k
x^k (均值,也就是 μ ),协方差矩阵
P
k
P_k
Pk 。(虽然还是用了位置和速度两个变量,但只要和问题相关,卡尔曼滤波可以包含任意数量的变量) 这是怎么做到的?为什么我们可以用矩阵来预测机器人下一刻的位置和速度?下面是个简单公式: 但是,除了速度和位置,外因也会对系统造成影响。比如模拟火车运动,除了列车自驾系统,列车操作员可能会手动调速。在我们的机器人示例中,导航软件也可以发出停止指令。对于这些信息,我们把它作为一个向量 u ⃗ k \vec{u}_k u k ,纳入预测系统作为修正。 假设油门设置和控制命令是已知的,我们知道火车的预期加速度 a 。根据运动学基本定理,我们可得: 当一个国家只按照自己的步子发展时,它会自生自灭。当一个国家开始依赖外部力量发展时,只要这些外部力量是已知的,我们也能预测它的存亡。 但是,如果存在我们不知道的力量呢?当我们监控无人机时,它可能会受到风的影响;当我们跟踪轮式机器人时,它的轮胎可能会打滑,或者粗糙地面会降低它的移速。这些因素是难以掌握的,如果出现其中的任意一种情况,预测结果就难以保障。 这要求我们在每个预测步骤后再加上一些新的不确定性,来模拟和“世界”相关的所有不确定性:
新的最佳估计 \color{magenta}{\text{新的最佳估计}} 新的最佳估计是基于 原最佳估计 \color{blue}{\text{原最佳估计}} 原最佳估计 和 已知外部影响 \color{orange}{\text{已知外部影响}} 已知外部影响 校正后得到的预测。 新的不确定性 \color{magenta}{\text{新的不确定性}} 新的不确定性 是基于 原不确定性 \color{blue}{\text{原不确定性}} 原不确定性 和 外部环境的不确定性 \color{turquoise}{\text{外部环境的不确定性}} 外部环境的不确定性 得到的预测。 现在,有了这些概念介绍,我们可以把传感器数据输入其中。 通过测量来细化估计值我们可能有好几个传感器,它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点,它可以读取位置,可以读取速度,重点是,它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。 现在我们得到了两块高斯分布,一块围绕预测的均值,另一块围绕传感器读数。 最简单的方法是两者相乘:
让我们从一维看起,设方差为 σ^2 ,均值为 μ ,一个标准一维高斯钟形曲线方程如下所示: 截至目前,我们有用矩阵
(
μ
0
,
Σ
0
)
=
(
H
k
x
^
k
,
H
k
P
k
H
k
T
)
(μ_0,Σ_0)=(H_k\hat{x}_k,H_kP_kH_{k}^{T})
(μ0,Σ0)=(Hkx^k,HkPkHkT) 预测的分布,有用传感器读数
(
μ
1
,
Σ
1
)
=
(
z
⃗
k
,
R
k
)
(μ_1,Σ_1)=(\vec{z}_k,R_k)
(μ1,Σ1)=(z
k,Rk) 预测的分布。把它们代入上节的矩阵等式中: 简单kalman滤波器的实现: class KalmanBoxTracker(object): #Kalman滤波的实现 """ This class represents the internel state of individual tracked objects observed as bbox. """ count = 0 def __init__(self,bbox): """ Initialises a tracker using initial bounding box. """ #define constant velocity model self.kf = KalmanFilter(dim_x=7, dim_z=4) self.kf.F = np.array([[1,0,0,0,1,0,0],[0,1,0,0,0,1,0],[0,0,1,0,0,0,1],[0,0,0,1,0,0,0], [0,0,0,0,1,0,0],[0,0,0,0,0,1,0],[0,0,0,0,0,0,1]]) self.kf.H = np.array([[1,0,0,0,0,0,0],[0,1,0,0,0,0,0],[0,0,1,0,0,0,0],[0,0,0,1,0,0,0]]) self.kf.R[2:,2:] *= 10. self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities self.kf.P *= 10. self.kf.Q[-1,-1] *= 0.01 self.kf.Q[4:,4:] *= 0.01 self.kf.x[:4] = convert_bbox_to_z(bbox) self.time_since_update = 0 self.id = KalmanBoxTracker.count KalmanBoxTracker.count += 1 self.history = [] self.hits = 0 self.hit_streak = 0 self.age = 0 def update(self,bbox): """ Updates the state vector with observed bbox. """ self.time_since_update = 0 self.history = [] self.hits += 1 self.hit_streak += 1 self.kf.update(convert_bbox_to_z(bbox)) def predict(self): """ Advances the state vector and returns the predicted bounding box estimate. """ if((self.kf.x[6]+self.kf.x[2])0): self.hit_streak = 0 self.time_since_update += 1 self.history.append(convert_x_to_bbox(self.kf.x)) return self.history[-1] def get_state(self): """ Returns the current bounding box estimate. """ return convert_x_to_bbox(self.kf.x) |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |