一维卡尔曼滤波
让我们来看一个在一维世界各地移动的机器人。 当机器人在世界中移动时，它通过执行以下循环来定位自己：
- 感知和执行测量更新
- 移动并执行动作更新
实现此滤波后，可以看到从非常不确定的高斯位置变为越来越确定的高斯，如下图所示。
以下是常规的高斯方程和实现
# 导入数学和显示模块
from math import *
import matplotlib.pyplot as plt
import numpy as np
# 高斯函数
def 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
接下来
-
update代码，该代码在合并初始belief和新测量信息时执行参数更新。 -
predict代码在动议结合后对Gasuuain进行更新。
# update函数
def update(mean1, var1, mean2, var2):
''' This function takes in two means and two squared variance terms,
and returns updated gaussian parameters.'''
# 计算新的值
new_mean = (var2*mean1 + var1*mean2)/(var2+var1)
new_var = 1/(1/var2 + 1/var1)
return [new_mean, new_var]
def predict(mean1, var1, mean2, var2):
''' This function takes in two means and two squared variance terms,
and returns updated gaussian parameters, after motion.'''
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
对于给定的测量和动作，编写完整的一维卡尔曼滤波代码，按顺序循环遍历所有这些。
完整代码应该查看传感器测量值，然后查看该序列中的运动，直到完成所有更新！
初始不确定状态
初始参数包括初始位置估计mu，和平方方差sig。 注意，初始估计设置为位置0，方差非常大; 这是一种高度混乱的状态，就像我们在直方图滤波器中使用的均值分布一样。 对于与传感器测量和运动相关的平方方差也给出了值，这些读数都不是完美的。
即使初始估计的位置（最初的mu）远离第一次测量，它也应该在循环测量和运动时相当快地赶上。
# measurements for mu and motions, U
measurements = [5., 6., 7., 9., 10.]
motions = [1., 1., 2., 1., 1.]
# initial parameters
measurement_sig = 4. #初始测量方差
motion_sig = 2. # 初始运动方差
mu = 0. #初始估计位置
sig = 10000. # 初始平方测量方差
## TODO: Loop through all measurements/motions
## Print out and display the resulting Gaussian
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))
print('\n')
print('Final result: [{}, {}]'.format(mu, sig))
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]
## Print out and display the final, resulting Gaussian
## 打印出最终的高斯分布
# set the parameters equal to the output of the Kalman filter result
mu = mu
sigma2 = sig
# define a range of x values
x_axis = np.arange(-20, 20, 0.1)
# create a corresponding list of gaussian values
g = []
for x in x_axis:
g.append(f(mu, sigma2, x))
# plot the result
plt.plot(x_axis, g)
显示一个高斯分布
通过循环一系列x值并创建高斯值的结果列表g来绘制高斯，如下所示。
# display the *initial* gaussian over a range of x values
# define the parameters
mu = 0
sigma2 = 10000
# define a range of x values
x_axis = np.arange(-20, 20, 0.1)
# create a corresponding list of gaussian values
g = []
for x in x_axis:
g.append(f(mu, sigma2, x))
# plot the result
plt.plot(x_axis, g)
