卡尔曼滤波器的python实现

2019-03-10 23:06发布

站内文章 / 后端开发

125 0

再贱就再见

女 | 书童

私信

本文将简化卡尔曼滤波器。希望你能学习并揭开你在学习卡尔曼过滤器中让你感觉到神秘的东西。

要了解卡尔曼滤波器，我们需要了解基础知识。在卡尔曼滤波器中，分布由所谓的高斯分布给出。

什么是高斯分布

高斯是位置空间上的连续函数，下面的区域总和为1。

高斯的特征在于两个参数，平均值，通常缩写为希腊字母μ（Mu），以及高斯的宽度，通常称为方差σ2（Sigma square）。因此，我们任务是保持μ和σ2平方作为我们试图找到的对象位置的最佳估计。

高峰分布的均值和方差

确切的公式是二次函数的指数，其中我们取表达式的指数。现在在下面的等式中，如果x等于μ，则分子变为0。事实证明，我们必须通过常数1除以2πσ2的平方根来对其进行归一化。

什么是方差？

方差是高斯扩散的度量（即曲线下面积的扩展）。较大的方差对应于较短的高斯。方差也是一种确定性的衡量标准; 如果你试图找到最确定的汽车位置之类的东西，你会想要一个高斯汽车，其平均值是汽车的位置，并且具有最小的不确定性/传播。

代码中的高斯函数实现

如何更新测量？

在卡尔曼滤波器中，我们迭代测量（测量更新）和运动（预测）。更新将使用贝叶斯规则，这不过是点乘或乘法。在预测中，我们使用总概率，即卷积或简单的加法。

测量更新和预测周期

执行测量周期然后预测周期如下：

1. 假设你正在本地化另一辆车，并且你有一个先验分布，如下所示; 它是一个非常宽的高斯平均值。这是我们之前的一个例子，我们对位置相当不确定，但测量告诉了我们很多关于车辆的位置。

2. 最终均值在两个旧均值，先验均值和测量均值之间移动。它在测量方面略微进一步，因为测量比先前测量有更确定的车辆位置。我们越确定，我们就越能在某个答案的方向上取平均值。

均值的本地化（绿色）

如何更新参数？

1. 假设我们乘以两个高斯，如贝叶斯规则，先验和测量概率。先验具有Mu的均值和Sigma平方的方差，并且测量具有Nu的平均值和r平方的协方差。

2. 然后，新均值Mu prime是旧均值的加权和。Mu由r平方加权，Mu由Sigma平方加权，由加权因子的总和标准化。新的方差项将是Sigma square prime。

3. 显然，先验高斯具有更高的不确定性，因此Sigma平方更大，这意味着nu的加权远大于Mu。因此，平均值将比mu更接近nu。有趣的是，方差项不受实际方法的影响，它只使用以前的方差。

更新参数

它在代码中的实现如下：

如何实现高斯运动？

1. 一个新的意思，是你的旧意思加上通常称为你的动作。所以，如果你在x方向移动超过10米，这将是10米，你知道西格玛广场是你的旧西格玛广场加上高斯运动的方差。这就是你需要知道的，这只是一个补充。

2. 在预测步骤中得到的高斯只是将这两个东西加起来，mu加上u和sigma square加r square。

运动更新/预测功能

它在代码中的实现如下：

# the mothon update/predict function
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 = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]

所以现在让我们把所有东西放在一起。让我们编写一个主程序，它接受这两个函数，更新和预测，并输入一系列测量和运动。在我选择的例子中：5.，6.，7.，9和10的测量值是1.，1.，2.，1.，1。

如果初始估计为5，那么这一切都会很好，但我们将其设置为0，具有非常大的不确定性10,000。假设：测量不确定度为常数4，运动不确定性为常数2。

for n in range(len(measurements)):
 # measurement update, with uncertainty
 mu, sig = update(mu, sig, measurements[n], measurement_sig)
 print('Update: [{}, {}]'.format(mu, sig))
 # motion update, with uncertainty
 mu, sig = predict(mu, sig, motions[n], motion_sig)
 print('Predict: [{}, {}]'.format(mu, sig))

当你运行这个时，你对位置的第一次估计应该基本上变成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。

Update: [4.998000799680128, 3.9984006397441023]
Predict: [5.998000799680128, 5.998400639744102]
Update: [5.999200191953932, 2.399744061425258]
Predict: [6.999200191953932, 4.399744061425258]
Update: [6.999619127420922, 2.0951800575117594]
Predict: [8.999619127420921, 4.09518005751176]
Update: [8.999811802788143, 2.0235152416216957]
Predict: [9.999811802788143, 4.023515241621696]
Update: [9.999906177177365, 2.0058615808441944]
Predict: [10.999906177177365, 4.005861580844194]
Final result: [10.999906177177365, 4.005861580844194]