课程4： Kalman Filters
2.练习：Tracking Intro
卡尔曼滤波，是一个非常流行的系统状态的估计方法，和概率定位方法相当类似，
与蒙特卡洛定位方法最主要的区别是：卡尔曼滤波对一个连续的状态进行估计，而蒙特卡洛定位方法中，得把世界分割成离散的小块。
作为结果，卡尔曼滤波给我们一个单峰分布。而蒙特卡洛定位方法是多峰分布。这两种方法都适用于机器人定位和对其他车辆的跟踪。
粒子滤波也是一种解决这类问题的方法。粒子滤波是连续且多峰分布的。
3. 练习：Gaussian Intro
直方图分布中，仅仅是对连续分布的近似表达。
卡尔曼滤波中，分布取决于所谓的高斯函数 Gaussian,高斯函数下面的面积加起来等于1. 一维高斯函数中，有2个变量，均值和方差。那么卡尔曼滤波中任务就是对一个未知的物体位置求出最佳的均值和方差。
4. 练习：Variance Comparison
方差越大，高斯函数波形越平缓，方差越小，高斯函数波形越窄
5. 练习：Preferred Gaussian
方差小，波形窄的高斯函数更好的。
6. 练习：Evaluate Gaussian
7. 练习：Maximize Gaussian
高斯函数的python简单实现代码：
from math import *
def f(mu, sigma2, x):
return1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)
print f(10.,4.,10.) #Change the 8. to something else!
X=均值时，高斯函数获得最大的值。
8. 练习：Measurements and Motion 1
测量时，我们是使用了乘法。移动时，我们是使用了卷积。
9. 练习： Measurements and Motion 2
测量时，使用的是贝叶斯定理，移动时，使用的是全概率定理
10. 练习：Shifting the Mean
先验概率分布假如是一个高斯函数的话，
如果测量得到了另一个高斯分布，则后验概率分布的高斯函数的均值位于前2个高斯函数均值之间。
11. 练习: Predicting the Peak
对于10中的情况，后验概率分布的峰值应该比前两个高斯分布的峰值更高。
12. 练习：Parameter Update
提到了2个高斯函数的波形，分别代表一个测量前，一个测量后。如果依据贝叶斯定理，对这2个高斯函数进行相乘，会得到一个新的高斯函数。新的高斯函数的均值和方差，可以由前面的2个高斯函数的均值和方差计算得来。计算公式的实现见16中的代码。
13. 练习：Parameter Update 2
14. 练习：Separated Gaussians
15. 练习： Separated Gaussians 2
16. 练习：New Mean and Variance
12中提到的计算方法的代码化，如下：
def update(mean1, var1, mean2, var2):
new_mean = (mean1*var2 +mean2*var1)/(var1+var2)
new_var = 1/(1/var1 +1/var2)
return [new_mean, new_var]
print update(10.,8.,13., 2.)update的四个参数是2个高斯函数的均值和方差
17. 练习：Gaussian Motion
运动时，本身也存在误差，这个误差也可以表示为一个高斯函数。那么，运动后的高斯函数计算方式，是原高斯函数 + 运动误差的高斯函数。
具体为 U2 = U1 + v, Var2 = Var1 + var
18. 练习：Predict Function
19. 练习：Kalman Filter Code
简单的一维卡尔曼过滤器，代码如下：
# -*- coding: utf-8 -*-
# Write a program that will iteratively update and
# predict based on the location measurements
# and inferred motions shown below.
def update(mean1, var1, mean2, var2):
new_mean = float(var2 *mean1 + var1 * mean2) / (var1 + var2)
new_var = 1./(1./var1 +1./var2)
return [new_mean, new_var]
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2.
mu = 0.
sig = 10000.
#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig].
# Insert code here
for i in range(len(motion)):
[mu, sig] =update(measurements[i], measurement_sig, mu, sig)
#print('after update [{},{}]'.format(mu, sig))
[mu, sig] = predict(mu,sig, motion[i], motion_sig)
#print('after predict[{},{}]'.format(mu, sig))
print('====================')
print([mu, sig])输出结果如下：
====================
[10.999906177177365, 4.005861580844194]
20. 练习：Kalman Prediction
卡尔曼过滤器，即使没有测量物体的速度，也可以通过物体的位置变化，得到物体的速度，进而预测追踪的物体以该速度出现的下一个位置。
21. Kalman Filter Land
多维高斯函数 或 多元高斯函数
对于n维空间来说，此时，均值是个nx1的向量，每个元素对应一个变量。
方差是协方差，是一个nxn的矩阵
多维高斯函数有专门的公式描述其函数值，跟均值和协方差还有当前的向量x有关。
22. 练习：Kalman Filter Prediction
在一维空间，已知当前位置，和速度的高斯函数，假设一个当前速度，可预测到后一时刻的位置和速度。
23. 练习：Another Prediction
24. More Kalman Filters
卡尔曼过滤器能通过已有的变量，观察到隐藏的变量。比如无人驾驶车能通过被追踪的其他汽车的不同时刻的位置，从而获得它们的速度。
25. Kalman Filter Design
1.状态转换函数
表示从当前状态到后一个状态的过程
Nx1 的新状态向量 = nxn 状态转换矩阵 * nx1 的当前状态向量
nxn 状态转换矩阵 被称为 F （在后续代码中）
2. 测量函数
Z = 1xn 矩阵 * nx1状态向量
这里的 1xn矩阵被称为 H（在后续代码中）
3. 卡尔曼滤波器的实际更新方程
下面讲一些代码中会出现的相关变量名的含义：
x 估计值
P 不确定性协方差矩阵
F 状态转换矩阵
u 移动向量
Z 测量值
H 测量方程
R 测量噪声
K 卡尔曼增益
I 单位矩阵
预测(prediction)过程：
x’ = F*x + u 预测新的x值
p’ = F*P*FT 预测新的P值，其中FT表示F的转置矩阵
测量更新过程（measurementupdate）：
y = Z – H*x y表示误差（error）
S = H*P*HT + R y被映射到S中
K = P*HT*S-1 K通常被称为卡尔曼增益，S-1 是S的反转矩阵
x’ = x + (K * y)
p’ = (I – K*H)*P
26. 练习：Kalman Matrices
具体代码实现例子，一个一维卡尔曼过滤器的代码：
# -*- coding:utf-8 -*-
# Write afunction 'kalman_filter' that implements a multi-
# dimensionalKalman Filter for the example given
from math import*
class matrix:
# implements basic operations of a matrixclass
def __init__(self, value):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
def zero(self, dimx, dimy):
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError("Invalid sizeof matrix")
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0 for row in range(dimy)]for col in range(dimx)]
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError("Invalid sizeof matrix")
else:
self.dimx = dim
self.dimy = dim
self.value = [[0 for row inrange(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1
def show(self):
for i in range(self.dimx):
print(self.value[i])
print(' ')
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy!= other.dimy:
raise ValueError("Matricesmust be of equal dimensions to add")
else:
# add if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] =self.value[i][j] + other.value[i][j]
return res
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy!= other.dimy:
raise ValueError("Matricesmust be of equal dimensions to subtract")
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] =self.value[i][j] - other.value[i][j]
return res
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError("Matricesmust be m*n and n*p to multiply")
else:
# multiply if correct dimensions
res = matrix([[]])
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] *other.value[k][j]
return res
def transpose(self):
# compute transpose
res = matrix([[]])
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] =self.value[i][j]
return res
# Thanks to Ernesto P. Adorio for use ofCholesky and CholeskyInverse functions
def Cholesky(self, ztol=1.0e-5):
# Computes the upper triangularCholesky factorization of
# a positive definite matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for kin range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raiseValueError("Matrix not positive-definite")
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] *res.value[k][j] for k in range(self.dimx)])
if abs(S) < ztol:
S = 0.0
try:
res.value[i][j] =(self.value[i][j] - S)/res.value[i][i]
except:
raise ValueError("Zerodiagonal")
return res
def CholeskyInverse(self):
# Computes inverse of matrix given itsCholesky upper Triangular
#decomposition of matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k]for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/tjj**2 -S/tjj
for i in reversed(range(j)):
res.value[j][i] =res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
return res
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
def __repr__(self):
return repr(self.value)
########################################
# Implement thefilter function below
def kalman_filter(x, P):
for n in range(len(measurements)):
# measurement update
Z = matrix([[measurements[n]]])
y = Z - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
# prediction
x = (F * x) + u
P = F * P * F.transpose()
# Test code
print('x :')
x.show()
print('----------------------')
print('P :')
P.show()
return x,P
############################################
### use the codebelow to test your filter!
############################################
measurements =[1, 2, 3]
x =matrix([[0.], [0.]]) # initial state (location and velocity)
P =matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u =matrix([[0.], [0.]]) # external motion
F = matrix([[1.,1.], [0, 1.]]) # next state function
H = matrix([[1.,0.]]) # measurement function
R =matrix([[1.]]) # measurement uncertainty
I = matrix([[1.,0.], [0., 1.]]) # identity matrix
print(kalman_filter(x,P))
# output shouldbe:
# x:[[3.9996664447958645], [0.9999998335552873]]
# P:[[2.3318904241194827, 0.9991676099921091], [0.9991676099921067,0.49950058263974184]]
当卡尔曼过滤从一维扩展到多维时，相关变量的维数相应会增加，
比如扩展到二维时，25中的例子里面，每个测量值 Z 为 [x,y] 的表示二维空间中的位置的值，而状态值 x 会从 2x1两个量变化为 4个量，分别表示x维度的位置和速度，y维度的位置和速度。。。
另一个关于卡尔曼过滤的在线课程地址 这个课程的视频讲到了多维卡尔曼过滤，后面的一部分讲到了追踪飞机的例子。。应该比 Udacity的这个要详细一些。这个链接的网页可以打开，但是播放视频不了，因为它是youtube的链接，被大陆屏蔽了。。