读书笔记-学习《Kalman-and-Bayesian-Filters-in-Python》

链接:rlabbe/Kalman-and-Bayesian-Filters-in-Python

github上只是一个介绍,真正的交互式阅读方法,是点击"launch binder"的图片按钮,就进入到

hub.mybinder.org/user/r

这里,列出了所有的章节,点击任何一个章节,进入该章节,就可以阅读并在线修改代码,在线运行。在文本框中修改代码后,按Ctrl+Enter即可运行。

上述方法比较慢,有时候会死。此外,还有一种方法可以快速看(仅仅看不能在线修改代码),即将.ipynb文件的url复制到nbviewer.jupyter.org/页面中的框中。

如何使用作者的成果:

1)预先安装NumPy,SciPy,matplotlib。这都是一些开源标准库。

2)安装作者的主要成果,FilterPy。这是一个了不起的多种类滤波器集合。

安装方法:在Ubuntu中,sudo pip install filterpy。

使用方法:from filterpy.xxx import yyy即可。xxx是某个子模块(例如卡尔曼子模块),yyy是某个类,例如卡尔曼滤波器类。

3)下载本书。以便运行书中的例子。

本书中的代码在kf_book子目录下,它里面包含了作者写的一些用于显示结果的图表类。

如果要写一个测试例程test.py,则可以把kf_book目录复制到test.py的同级目录下,然后例如from kf_book.mkf_internal import plot_track的方式引用其中的图表类。

00-Preface 前言

这章很简单,就是介绍numpy scipy等基础知识。

01-The g-h Filter g-h滤波

用一个体重变化的例子,来讲述g-h滤波。它是所有滤波的基础,卡尔曼滤波等等都是它的一种。

g-h滤波实际上是一个套路,它的过程是:

1)预测一步。

2)利用测量值更新一步。再回到1。

g-h滤波中有2个参数,g是测量值的权重,h是预测值的权重。

一种算法的描述是:

初始化

1。初始化过滤器的状态

2。初始化我们对状态的信念

预测

1。使用系统行为预测下一步的状态

2。调整信念来考虑预测的不确定性

更新

1。得到一个测量值和其精度的信念

2。估计状态与测量之间的残差

3。新估计是在残差线上的某个地方

02-Discrete Bayes Filter 离散贝叶斯滤波

卡尔曼滤波器属于一个称为贝叶斯滤波器的滤波器族。

这里距离一个测量狗狗在走廊哪个位置的例子,来说明“预测”-“更新”模型是如何运作的。

引入了卷积,作为概率预测模型,下面就是一个核为[0.1,0.7,0.2]的离散概率扩散模型,意思是,每个位置的概率质量,在下一时刻,0.8的概率来自自己,0.2的概率来自左右邻居。

from filterpy.discrete_bayes import predict

belief = [.05, .05, .05, .3, .55, .3, .05, .05, .05, .05]

prior = predict(belief, offset=1, kernel=[.1, .8, .1])

从结果[0.05 0.05 0.05 0.1 0.4 0.15 0.05 0.05 0.05 0.05]上看,右边概率大一些,所以,predict是一个概率推的模型,不是一个拉的模型。

这里又引入了一个火车在轨道上行走的例子,这个例子更加通用,在每次迭代中,做以下代码:

robot.move(distance=move_distance) #操纵火车向前走4步

prior = predict(posterior, move_distance, kernel) #用自带的predict()预测4步之后的概率质量,其中kernel=[0.1,0.8,0.1],表示一步的概率离散程度。

m = robot.sense() #测量一下车辆所在的位置,注意这个函数有9成正确率,一成错误1格的概率。

likelihood = lh_hallway(track, m, sensor_accuracy) #利用测量得到的m,对所有的离散位置,计算车辆再此处的可能性。

posterior = update(likelihood, prior) #利用自带的update()函数,对概率质量进行一次贝叶斯后验迭代。

我用自己的语言来描述依稀离散贝叶斯滤波:

预先设定

狗只能在n个位置。

狗在每个位置的概率,用一个概率质量函数来描述。所以,这个概率质量函数,用来描述狗的位置。

1)初始化,给出初始的概率质量函数,来描述狗的位置。

2)预测狗向右走一步,就是调用一下预测动作。就是把用来描述狗位置的概率质量函数,变化一下。一般情况下,概率质量会变的更加扁平(模糊)。

3)测量一下狗的位置,得到的是一个确定的位置值,虽然这个位置仅仅有一定精确度。

4)利用这个位置值和测量精确度,我们计算出每个位置的likelihood。

5)利用这个likelihood,我们用贝叶斯法则,更新一下概率质量函数。

6)转到2)

这本书主要是关于卡尔曼滤波器。它使用的数学是不同的,但是逻辑与本章中使用的完全一样。它使用贝叶斯推理来从测量和过程模型的组合中形成估计。

03-Gaussian Probabilities 高斯分布

用scipy.stats

两个正态分布相加,还是一个正态分布,μ = μ1 + μ2,σ^2 = σ1^2 + σ2^2

两个正态分布相乘,还是一个正态分布,μ = (σ1^2 * μ2 + σ2^2 * μ1)/(σ1^2 + σ2^2), σ^2 = (σ1^2 * σ2^2) / (σ1^2 + σ2^2)

04-One Dimensional Kalman Filters 一维卡尔曼滤波

from numpy.random import randn

randn(25)将返回一个25个元素的数组,填充了从均值为0和方差为1的单变量“正态”(高斯)分布中采样的随机浮点数。

randn()则返回一个数字,均值为0和方差为1的一个随机采样。

randn(3,5)则返一个矩阵,填充了均值为0和方差为1的二维高斯分布中的随机采样浮点数。

一个非常简单有趣的高斯分布预测:

我的狗在10米处,方差是0.2米,我们记为N(10,0.2**2);

狗的速度是15米/秒,每秒的标准误差07米,则每秒移动的距离可以记为N(15,0.7**2);

那么,狗在一秒之后的位置就是 N(10+15,0.2**2+0.7**2)。

卡尔曼增益

K = σ1^2/(σ1^2 + σ2^2) K就是卡尔曼增益,

用来表示平均值:μ = K * μ2 + (1 - K) * μ1 = μ1 + K * (μ2 - μ1)

这是Kalman滤波器的关键所在。K是一个缩放项,它选择一个介于μ2和μ1之间的值。

举个例子,当测量值很准,即方差比先验值方差小9倍时,σ2^2 = 9 * σ1^2, K = 0.9,则μ = 0.9*μ2 + 0.1*μ1,即后验值非常接近测量值。

用来表示方差:σ^2 = (1 - K) * σ1^2

这里,当K越大(测量值越精确),新的方差就越小。

卡尔曼代码

def update(prior, measurement):

x, P = prior # mean and variance of prior

z, R = measurement # mean and variance of measurement

y = z - x # residual

K = P / (P + R) # Kalman gain


x = x + K*y # posterior

P = (1 - K) * P # posterior variance

return gaussian(x, P)


def predict(posterior, movement):

x, P = posterior # mean and variance of posterior

dx, Q = movement # mean and variance of movement

x = x + dx

P = P + Q

return gaussian(x, P)

从上述卡尔曼增益计算新的平均值的公式上,可以看出,卡尔曼滤波就是g-h滤波的一种。

下面是作者写的卡尔曼类的使用例子

import filterpy.kalman as kf

x, P = kf.predict(x=10., P=3., u=1., Q=2.**2)

x, P = kf.update(x=x, P=P, z=12., R=3.5**2)

05-Multivariate-Gaussians 多维高斯

numpy.cov()获得协方差矩阵,注意,它是无偏估计的。

多维正态分布的方程,和一维的类似。SciPy的STATS模块的multivariate_normal()实现了多维正态方程

import scipy

mu = [2.0, 7.0] # 狗位置是二维正态分布,均值在x=2, y=7处

P = [[8., 0.],

[0., 3.]] # 狗的协方差矩阵,x方向上不确定性大一些,y方向上不确定性小一些,但xy之间无关联

x = [2.5, 7.3] #

print('{:.4f}'.format(scipy.stats.multivariate_normal(mu, P).pdf(x)))

# 得到 0.0315, 表示在(2.5, 7.3)这个点,概率密度的值是0.0315

书中用等高线来描述二维高斯分布的形状。

皮尔森系数 ρxy = COV(X,Y) / (σx * σy) 这个值在-1到1之间,为0时表示x y不相关,越接近-1表示越是负相关,越接近1表示越是正相关。

用皮尔森系数可以解释等高线椭圆的瘦度--当ρxy接近-1或1时,椭圆越瘦。

例如,当σx^2=1 σy^2=2时,COV(X,Y)=1.414时,椭圆最瘦,接近一个直线。

我们可以使用SIPY.STATS.PARSONR函数来计算皮尔森系数

多维高斯卡尔曼滤波公式,和一维的类似:

μ = Σ2(Σ1+Σ2)^−1 μ1 + Σ1(Σ1+Σ2)^−1 μ2

Σ = Σ1(Σ1+Σ2)^−1 Σ2

这里提出了一个雷达测飞机二维位置的例子。雷达测方位很准但测距离不准,所以,测出的位置其实就是一个椭圆的正态分布,用一个协方差矩阵表示。

开始飞机是一个大圆。

左下角有一个雷达,测一下,得到一个东北西南向的椭圆,椭圆和大圆相乘,得到更小的椭圆,为更新值。

右下角又有一个雷达,测一下,得到一个西北东南向的椭圆,该椭圆和之前更新的东北西南向的小椭圆相乘,得到一个阴影相交部分的很小的近似圆形的椭圆。

2个雷达最好是正交的,这里有一个非正交的例子,会发现得到的阴影重叠部分变大,不利于缩小范围。

二维高斯的误差,我们用椭圆表示;三维高斯的误差,可以用椭球表示。

06-多维卡尔曼滤波

过程

Initialization

1. Initialize the state of the filter

2. Initialize our belief in the state Predict

1. Use process model to predict state at the next time step

2. Adjust belief to account for the uncertainty in prediction Update

1. Get a measurement and associated belief about its accuracy

2. Compute residual between estimated state and measurement

3. Compute scaling factor based on whether the measurement or prediction is more accurate

4. set state between the prediction and measurement based on scaling factor

5. update belief in the state based on how certain we are in the measurement

公式

预测

x1 = F x + B u

P1 = F P T(F) + Q

x,P是均值和协方差矩阵. 对应着一维卡尔曼的x和σ2.

F是状态转移函数。当乘以x时,它计算先验。

Q是过程协方差。

更新

y = z - H x1

K = P1 T(H) (H P1 T(H) + R)^-1

x = x1 + K y

P = (I - K H) P1

H是测量函数。如果你从等式中移除HH,你应该能够看到这些方程也是相似的。

z、R是测量均值和噪声协方差。

y、K是残差和卡尔曼增益

这个公式的含义和一维卡尔曼的意义完全相同

用高斯表示状态和误差的估计

用高斯表示测量及其误差

用高斯表示过程模型

使用过程模型预测下一个状态(先验)

在测量与先验之间形成一个估计部分

你作为设计师的任务是设计状态(X,P),过程(F,Q),测量(Z,R),和测量函数H。如果系统有控制输入,例如机器人,你也将设计B和U。

一个追踪狗的例子

狗有2个状态:位置和速度

我们设计狗的初始状态:

x = [10, 4.5] # 位于10米处,速度为4.5

P = np.diag([500., 49.]) #diag()生成对角矩阵。500表示我们对初始位置的不确信;49是因为,我们假设狗最大速度是21m/s,取3σ=21, σ=7, σ^2=49

过程模型设计:

参考:scipy.linalg.solve()可以用来解简单的矩阵方程Ax=B

注意,x1 = F x, 我们作为卡尔曼滤波器设计者的工作是指定F,使得x1 == F x,对我们的系统进行预测。

我们认为 x1 = x + v * dt

v1 = v

所以有, F = [ [1, dt], [0, 1] ]

关于白噪声,我们专门写了一个函数,来计算:

from filterpy.common import Q_discrete_white_noise

Q = Q_discrete_white_noise(dim=2, dt=1., var=2.35) # 得到一个二维的,1秒的,方差是2.35的白噪声矩阵

此外,我们的狗没有任何控制输入,所以B=0, u=0,当然,0也是他们的默认值。

总结:我们作为设计师,任务就是指定矩阵。

X,P:状态和协方差

F、Q:过程模型和噪声协方差

B,u:可选地,控制输入和函数

测量函数的设计

我们的工作,是设计2个矩阵。

测量空间

有时候测量空间和计算空间不一致。例如,我们需要测电压,但需要通过温度来简介测量电压。

大部分情况下,测量是不可逆的,例如,可以把电压转成温度值,但不能把预测的温度值反过来转成电压值来进行计算。

又比如,考虑给目标提供范围的传感器。将一个范围转换成一个位置是不可能的——一个圆中无限数量的位置将产生相同的范围。

在我们的追踪狗的例子中,测量只能得到位置值。所以,测量空间是1维的,而预测空间是2维的。

预测值x和测量值z都是向量,所以我们需要一个转换来计算残差:

y=z−Hx

其中y为残差,x为先验,z为测量,H为测量函数。因此,我们取先验,将其转换为测量,通过与H相乘,并从测量中减去。

我们的状态x有2个值[位置、速度],但我们的传感器只能测位置,所以我们的z是一个只有一个元素的向量[z]

因为,y=z-Hx,所以,H就必须是一个[? ?]的一行二列的矩阵了。

我们测量得到的位置和预测的位置是一致的,因此乘以1就可以得到,而我们测量不到速度,所以就乘以0,所以 H=[1 0]

设计测量

我们测量的z是均值,还要有测量的协方差矩阵R。

z很容易,我们只测量到了位置,所以z=[位置]。如果,我们能同时得到速度的话,z=[位置, 速度]

协方差矩阵R具有m*m的尺寸,其中m是传感器的数量。这是一个协方差矩阵来解释传感器之间的相关性。我们只有1个传感器,所以R是:[σ^2]

现在,我们假设这个传感器的的方差是5,所以R = [5]

如果,我们有了2个独立的位置传感器,第一个方差为5,第二个方差为3,我们会写:

R = [[5 0], [0, 3]]

使用例子

from filterpy.kalman import KalmanFilter

KalmanFilter是作者封装的一个类,只要把各种矩阵都填好,就可以迭代它了。

更新方程

系统不确定性

看下面两个类似的表达式:

S = H P1 T(H) + R # 坐标系统线性变换引起的协方差的改变

P1 = F P T(F) + Q # 状态转换引起的协方差的改变

卡尔曼增益

K = P1 T(H) S^-1 # 0到1之间的实数

下面的方式,可以帮助理解K

K ≈ (P1 / S) T(H)

K ≈ (预测的不确定性 / 测量的不确定性) T(H)

所以,当测量非常精确时,K就很大,导致结果向测量值那边靠拢。

总的来说,K就是,将先验的不确定性与先验和测量的不确定性的总和相除。

残差

y = z − H x1

测量函数将一个预测值转换成一个测量值。然后用真的测量值减去它。

在我们的狗例子中,就是把2维的预测值转换成1维的测量值,然后用真的测量值减去它。

状态更新

x = x1 + K y

我们选择我们的新状态沿残差,按卡尔曼增益缩放。缩放由Ky来执行,Ky既对残差进行缩放,又用K中的T(H)项将其转换成状态空间。

协方差更新

P = (I − K H) P1

我们可以这样来理解它:P=(1−cK)P1,如果测量值更精确,就K大,则(1−cK)小,则更新的P就小(状态精确);反之,测量值不精确,就K小,则(1-cK)大,则更新的P就不够小(状态模糊)

一个不用FilterPy的定位狗的例子

dt = 1. # 每步1秒

R_var = 10 # 测量方差

Q_var = 0.01 # 预测方差

x = np.array([[10.0, 4.5]]).T # 开始时,在10米处,速度4.5

P = np.diag([500, 49]) # 开始时,位置方差500,速度方差49

F = np.array([[1, dt],

[0, 1]]) # 用牛顿公式定义转换矩阵

H = np.array([[1., 0.]]) # 只测量位置,H是把2维的预测值转换成为1维的测量值。

R = np.array([[R_var]]) # 测量方差

Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var) # 用预定义的方式,计算过程协方差矩阵

from numpy import dot

from scipy.linalg import inv


count = 50

track, zs = compute_dog_data(R_var, Q_var, count) # 获得随机生成的狗的位置测量值

xs, cov = [], []

for z in zs:

# predict

x = dot(F, x)

P = dot(F, P).dot(F.T) + Q

#update

S = dot(H, P).dot(H.T) + R

K = dot(P, H.T).dot(inv(S))

y = z - dot(H, x)

x += dot(K, y)

P = P - dot(K, H).dot(P)

xs.append(x)

cov.append(P)


xs, cov = np.array(xs), np.array(cov)

plot_track(xs[:, 0], track, zs, cov, plot_P=False, dt=dt)

过滤器初始化

一个好的办法是,在获取第一个测量值z后,用这个测量值作为初始化值x。

如果存在H,则x = H^-1 z。矩阵求逆需要一个正方形矩阵,但H很少是正方形的,所以我们可以用scipy.linalg.pinv()来求伪逆。

初始速度,可以用第一个测量值和第二个测量值的差计算出来。

P的初始值,其中位置的方差,可以用测量方差R;其中速度的方差,最大速度平方是一个合理的值。

成批处理

卡尔曼滤波器被设计为递归算法-随着新的测量进来,我们立即创建一个新的估计。但是有一组数据是很常见的,这些数据已经被收集,我们想要过滤。卡尔曼滤波器可以在批处理模式下运行,其中所有的测量都被立即过滤。我们已经在Kalman过滤器中实现了这一点。

一个例子如下:

count = 50

track, zs = compute_dog_data(10, .2, count)

P = np.diag([500., 49.])

f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)

xs, _, _, _ = f.batch_filter(zs)

平滑结果

有一种情况,需要用到之后发生的事情来对当前的信号做决定。例如,汽车行驶过程中,当GPS跳向左边,需要判断是否真左转了还是一个噪音。此时,就需要之后的几帧数据作为判定依据。

Kalman滤波器实现了该算法的一种形式,称为RTS平滑器 rts_smoother(),使用它通过从batch_filter()步骤计算出的均值和协方差,并接收平滑的均值、协方差和卡尔曼增益

一个例子如下:

from numpy.random import seed

count = 50

seed(8923)


P = np.diag([500., 49.])

f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)

track, zs = compute_dog_data(3., .02, count)

Xs, Covs, _, _ = f.batch_filter(zs)

Ms, Ps, _, _ = f.rts_smoother(Xs, Covs)


book_plots.plot_measurements(zs)

plt.plot(Xs[:, 0], ls='--', label='Kalman Position')

plt.plot(Ms[:, 0], label='RTS Position')

plt.legend(loc=4);

讨论与总结

多元高斯允许我们同时处理多个维度,包括空间和其他维度(速度等)。我们做了一个关键的见解:隐藏变量有能力显著提高过滤器的精度。这是可能的,因为隐藏变量与观察到的变量相关。

R和Q的设计通常很有挑战性。传感器常常不是高斯型,通常有胖尾(称为峰度)和歪斜。Q的情况更可怕,你不知道狗下一步可能怎样。

这些问题导致一些研究人员和工程师贬低地称Kalman过滤器为“泥球”。

另一个要知道的术语——卡尔曼滤波器会变得自鸣得意。

设计一个线性滤波器的矩阵是一个实验过程,而不是一个数学过程。使用数学来建立初始值,但是你需要进行实验。

07-卡尔曼滤波数学

动态系统的状态空间表示

由高阶方程生成一阶方程

我们最简单的动力学方程就是x'=v了,它可以转换成x'=Ax+Bu+w的形式。

卡尔曼提出了状态空间方法,对于高阶的动力学微分方程,把它转换成1阶微分方程组。

例如对微分方程 x''−6x'+9x=u

我们把最高次项拿到左边,变成 x''=6x'-9x+u

在令x1(u)=x x2(u)=x',我们知道x1'=x2, x2'=x'',所以就有 x2' = 6 x2 - 9 x1 + t,因此,我们的一阶方程组是

x1' = x2

x2'= 6 x2 - 9 x1 + t

如果你稍微练习一下,你就会熟练掌握它。隔离最高项,定义一个新的变量及其导数,然后替换。

状态空间形式的一阶微分方程

x1=x x1'=x2 x2'=x3 ... 微分方程最高阶是n的话,就可以转化成n次1阶微分方程组。就成为以下形式

x' = Ax+Bu

其中A是系统动力学矩阵

求时不变系统的基本矩阵

我们已经知道了A,如何求F?使F满足递推关系 new_x = F(Δt) x,Δt拿掉就可以简写为 new_x = F x

换言之,A是一组连续微分方程,我们需要F是一组离散线性方程组,计算离散时间步长中A的变化。

一般来说,有三种常用的方法来寻找卡尔曼滤波器的这种矩阵。最常用的技术是矩阵指数法。线性时不变理论,也称为LTI系统理论,是第二种技术。最后,有数值技术。

矩阵指数

先来解一个微分方程 x' = kx,得到x=c0 e^(kt),具体解的时候,可以利用在线积分计算器zh.numberempire.com/int

同理,当x为向量的时候,x'=Ax也可以解得x = e^(At) x0,所以,当F = e^(At)时,就有 new_x = F x。

e^(At)称之为矩阵指数。它可以用这个幂级数来计算:

e^(At) = I + At + (At)^2 / 2! + (At)^3 / 3! + ...

来个例子,匀速直线运动的牛顿方程:

T([x', v']) = [[0 1],[0 0]] * T([x, v])

上式中,A = [[0 1],[0 0]],表示x'=v, v'=0,是一个匀速直线运动。

F = e^(At) = I + At + + (At)^2 / 2! + ...

注意,上式中A^2 = [[0 0],[0 0]],所以,

F = e^(At) = I + At = [[1 0],[0 1]] + [[0 1],[0 0]]t = [[1 t],[0 1]],果然,和常识一致。

scipy.linalg.expm()可以计算矩阵指数。举例如下:

import numpy as np

from scipy.linalg import expm

dt = 0.1

A = np.array([[0, 1],

[0, 0]])

expm(A*dt)

# 结果是array([[ 1.0, 0.1],[ 0.0, 1.0]])

时不变

如果系统的行为依赖于时间,我们可以说动态系统是由一阶微分方程描述的

g(t) = x'

然而,如果系统是时不变的,方程的形式是:

f(x) = x'

时间不变量意味着什么?考虑一个家庭立体声。如果在时间t中输入一个信号x,它将输出一些信号f(x)。如果在时间t+Δt中执行输入,则输出信号将是相同的f(x)。

一个反例是x(t)=sin(t),f(x)=t x(t),此时,t不同的时候,同样的x,输出不同的f(x)。

我们可以通过积分各边来求解这些方程。 v=x'这个方程很好做,但x'=f(x)就较难了。

数值解

x'=Ax+Gw

C. F. van Loan开发了一种既能找到数值也能找到Q的技术。代码如下:

from filterpy.common import van_loan_discretization

A = np.array([[0., 1.], [-1., 0.]])

G = np.array([[0.], [2.]]) # white noise scaling

F, Q = van_loan_discretization(A, G, dt=0.1)

过程噪声矩阵的设计

一般来说,Q矩阵的设计是卡尔曼滤波器设计中最困难的方面之一。

这是由于几个因素造成的。首先,数学需要一个良好的信号理论基础。第二,我们试图在我们没有什么信息的情况下模拟噪声。

我们将考虑两种不同的噪声模型。

连续白噪声模型

我们使用牛顿方程来模拟运动学系统。然后我们可以假设加速度对于每个离散的时间步长是恒定的。换言之,我们假设速度的微小变化随着时间的推移平均为0(零均值)

分段白噪声模型

另一个模型假定,最高阶项(例如,加速度)在每个时间段的持续时间是恒定的,但对于每个时间段是不同的,并且每一个在时间段之间是不相关的。

使用过滤器计算Q

from filterpy.common import Q_continuous_white_noise

from filterpy.common import Q_discrete_white_noise

Q = Q_continuous_white_noise(dim=2, dt=1, spectral_density=1)


08-设计卡尔曼滤波

跟踪机器人

我们的例子,是跟踪一个在二维平面上移动的机器人。

选择状态变量

[x,y]太简单了。我们使用[x, vx, y, vy]。因为我们是每隔1秒测量一下,所以,vx就是两帧x的差,vy也类似。

设计状态转移函数F

x = 1x + Δt vx + 0y + 0 vy

vx = 0x + 1 vx + 0y + 0 vy

y = 0x + 0 vx + 1y + Δt vy

vy = 0x + 0 vx + 0y + 1 vy

所以,我们有

from filterpy.kalman import KalmanFilter

tracker = KalmanFilter(dim_x=4, dim_z=2) #预测是4维的,测量是2维的

dt = 1. # time step 1 second

tracker.F = np.array([[1, dt, 0, 0],

[0, 1, 0, 0],

[0, 0, 1, dt],

[0, 0, 0, 1]])

过程噪声矩阵Q的设计

FiTyPy可以为我们计算Q矩阵。为了简单起见,我假设噪声是离散时间Wiener过程——它对于每个时间周期是恒定的。如果不清楚,请重新访问卡尔曼滤波器数学章节。

from scipy.linalg import block_diag

from filterpy.common import Q_discrete_white_noise

q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)

tracker.Q = block_diag(q, q)

print(tracker.Q)

测量函数H的设计

我们假设,测量的单位是英尺而预测的单位是米。1英尺=0.3048米。

tracker.H = np.array([[1/0.3048, 0, 0, 0],

[0, 0, 1/0.3048, 0]])

测量噪声矩阵R的设计

我们假设,测量x,y方向上的误差是独立的,是5m2。

tracker.R = np.array([[5., 0],

[0, 5]])

初始条件

我们假设开始在[0,0]处,速度为0。这个假设不很有信心,所以方差很大。

tracker.x = np.array([[0, 0, 0, 0]]).T

tracker.P = np.eye(4) * 500.

实现过滤器

设计已经完成,现在我们只需编写代码来运行过滤器并输出我们选择的格式的数据。我们将运行代码30次迭代。

滤波器的阶

0阶,静态系统。例如测量静止物体的位置。

1阶,有一阶导数,比如速度。例如测量匀速行动的小车位置。

2阶,有二阶导数,比如加速度。例如测量匀加速运动的小车的位置。

我们模拟出一个一阶的测量数据。然后用0阶滤波器和2阶滤波器去滤波,结果发现,0阶始终落后,2阶虽然基本吻合,但残差比1阶大很多且有时无法收敛。

低阶滤波器可以跟踪更高阶的过程,只要增加足够的过程噪声,并且保持离散化周期小(100个采样每秒,通常是局部线性的)。

我们再次模拟出一个二阶的测量输出。然后用1阶的滤波器去滤波,我们把Q增大了10倍,结果效果还不错。

不良测量的检测与剔除

有时候,会得到偏离过远的测量值。这时候要考虑不要他。

首先,让我们介绍两个术语。我们正在讨论门控。门是一种公式或算法,它决定测量的好坏。只有好的测量才能通过大门。这个过程称为门控。

在实践中,测量不是纯高斯的,所以3个标准偏差的门很可能会丢弃一些好的测量结果。我将很快详细说明,现在我们将使用4个标准偏差。

通过计算马氏距离(一个样本距离中心的有几个标准差),来进行去除坏的测量结果。

门控与数据关联策略

有矩形门(计算压力小)、策略门(不太坏)、椭圆形门(计算压力大但更准确),此外还有各种门,例如汽车,将有一个小得多的2D饼状的形状突出在它前面。

如何选择门是一个艺术。在5个维度上,矩形门是接受椭球的坏测量的两倍。

如果需要快,并且坏数据多,可以采用2门法:第一个门是大矩形们,用来做粗过滤。第二个椭圆门,来做更昂贵的马氏距离计算。

归一化估计误差平方(NEES)

首先,只有在模拟系统里,才有真值,才有NEES

x = x_true - x_filtered 先得到真值和滤波结果的差

ϵ = T(x) P^−1 x 求出NEES,从式子上可以看出,P小会导致NEES变大。ϵ是一个标量。

NEES的意义是:取所有NEES值的平均值,它们应该小于X的维数。例如x是四维的如[x, vx, y, vy],那么NEES均值应该小于4才是好的滤波器。

提供了filterpy.stats.NEES。这是过滤器性能的一个极好的度量,并且应该尽可能使用,特别是在生产代码中,当您需要在运行时对过滤器进行评估时。

似然函数

滤波器的残差和系统不确定性定义为

y = z - H x

S = H P T(H) + R

然后用类似高斯方程的方法计算可能性

from scipy.stats import multivariate_normal

hx = np.dot(H, x).flatten()

S = np.dot(H, P).dot(H.T) + R

likelihood = multivariate_normal.pdf(z.flatten(), mean=hx, cov=S)

用plt.plot(s.log_likelihood);语句可以把一次滤波过程中的每帧的likelihood打印出来,如果likelihood变大则表示滤波器好,如果接近0则表示滤波器坏。

控制输入

x = F x + B u

这里用轨道上的机器人的例子来描述控制。如果发一个速度指令,机器人可以立即变化为指定的速度v1。那么,

x = x + v1 Δt

v = v1

则可以令 u = [v], B = T([Δt, 1])

这是一种简化;典型的控制输入是转向角的变化和加速度的变化。这引入了非线性,我们将在后面的章节中学习处理。

传感器融合

我们有两个尺度的滤波器,一个精确,一个不精确。我们应该如何将其纳入卡尔曼滤波器?

例1:假设一辆车上有GPS和速度脉冲(车轮转一圈给一个脉冲)。我们假设速度脉冲测量得到的也是绝对位置x,则令预测值为[x, v],测量值为[x_gps, x_wheel],即可。

例2:假设空间一个点在(100,100)处,我们有2个雷达,一个在(50,50)处,一个在(150,50)处。雷达的角度误差在0.5度,距离误差在3米。

练习:你能过滤GPS输出吗?

GPS的输出,其实是经过了过滤的。如果再过滤一次,会不会更好?这里做了一个实验,把GPS过滤一次,将结果作为输入,再过滤一次,再将结果作为输入,再过滤一次。

结果发现,得到的曲线,越来越平滑,仅此而已,并没有越来越正确。

这是因为,卡尔曼滤波要求输入是时间无关的。而滤波的结果,则是时间相关的。

非平稳过程

如果我们的数据率以某种不可预测的方式发生变化呢?或者,如果我们有两个传感器,每个传感器以不同的速率运行?如果测量误差发生了什么变化?

解决方案很简单,在滤波器迭代过程中,修改参数矩阵F或其他参数即可。

传感器融合:不同的数据速率

两个不同的传感器类以相同的速率输出数据是罕见的。假设位置传感器产生3 Hz的更新,并且车轮在7赫兹下更新。实验了一个匀速运动的轨道小车的例子。

这个代码是非常重要的,值得看看!

追踪球

一个例子,我们把一个球,斜向天空抛出。不考虑空气摩擦力,我们可以用Runge-Kutta方法计算出轨迹作为真值。然后用卡尔曼滤波。

选择状态变量:

我们用欧拉法代替Runge-Kutta法来写离散方程,因为足够精确且不需要编写自定义预测函数。这意味着我们需要将Y的加速度合并到卡尔曼滤波器中,而不是X。

所以,状态变量是:x=[x x' y y' y'']T

但因为重力是已知常数,所以,不需要y'',把他放到控制变量矩阵B中间即可。所以,状态变量是:x=[x x' y y']T

设计状态转移函数

F=[[1 dt 0 0],

[1 1 0 0],

[1 0 1 dt],

[1 0 0 1]]

控制输入功能的设计

我们将使用控制输入来解释重力的作用。把Bu这个词加到X中,以解释X因重力变化的多少。可以说,Bu包含[dx, dvx, dy, dvy]。

如果我们看离散方程,我们看到重力只影响Y的速度。因此,我们希望产品Bu等于[0 0 0 -g*dt]。

最终的设计 B = [0 0 0 dt]T u = [0 0 0 -g]

测量函数的设计

z = [x, y]T

H = [[1 0 0 0],

[0 0 1 0]]

测量噪声矩阵的设计

假设误差在x和y中是独立的,都为0.5米平方。

R = [[0.5 0],

[0 0.5]]

过程噪声矩阵的设计

我们假设一个球在真空中运动,所以不应该有过程噪声。我们有4个状态变量,所以我们需要一个4×4全0的协方差矩阵。

追踪一个空气中的球

这次,把空气阻力也算上了。有一个复杂的公式来生成球的新轨迹。新轨迹比老轨迹要低矮一些,球先落地。

但,还用上次的那个滤波器来滤波,发现,当R大的时候,相信预测值多一些,导致轨迹高了远了。当R小的时候,则轨迹偏的不太多。

为了解决这个问题,我们把球受到的阻力想象成过程噪声Q,原先Q为0,当Q为0.01的时候,轨迹好了一些,当Q为0.1的时候,轨迹和真值基本重合了。

当我们使过程噪声高于系统中的实际噪声时,滤波器将选择对测量结果进行更高的称量。如果你在测量中没有太多噪音,这对你可能有效。

然而,考虑下一个情节,我增加了测量中的噪声。结果就悲催了。

有了这一点,当然可以使用过程噪声来处理系统中的小非线性。这是卡尔曼滤波器的“黑色艺术”的一部分。

09-非线性滤波

讲述了几个非线性的例子,表线性卡尔曼滤波应用时无法收敛。

对于非线性问题,作者更加倾向于使用UKF(无迹卡尔曼滤波)而不是EKF(扩展卡尔曼滤波)。UKF已经取得了巨大的成功。

UKF和粒子滤波器。他们更容易实施,理解和使用,而且他们通常比EKF更稳定。

10-无迹卡尔曼滤波

x是高斯分布,但f(x)不是高斯分布。每次更新,我们生成500,000个点,将它们传递给函数,然后计算结果的均值和方差。这被称为蒙特卡洛方法。

它被一些卡尔曼滤波器设计所使用,例如集成滤波器和粒子滤波器。

这种方法确实有效,但它在计算上非常昂贵。集成滤波器和粒子滤波器使用巧妙的技术来显着降低这种维度,但计算负担仍然非常大。

无迹卡尔曼滤波器使用西格玛点,但通过使用确定性方法来选择点大大减少了计算量。

西格玛点 - 从分布中抽样

from numpy.random import multivariate_normal

快速示例

在线性卡尔曼滤波中,我们使用H作为测量到状态的转换函数。注意,这里说的是“函数”,所以,对于线性H就是一个矩阵,对于非线性,就是一个函数。

这里给出了一个例子,是一个线性的情况,但用函数代替H矩阵,用线性卡尔曼滤波器来做滤波。

选择Sigma点

如何选择,有无数种办法。更一般的方法是计算加权平均值。即选择若干个西格玛点,并给每个西格玛点加上权值wm(用于计算均值),wc(用于计算协方差矩阵)

无迹变换

目前,假设存在用于选择西格玛点和权重的算法。西格马点如何实现过滤器?

简而言之,无迹变换需要从一些任意的概率分布中抽取点,并将它们传递给一个任意的非线性函数,并为每个变换点产生一个高斯函数。我希望你可以设想我们如何使用它来实现非线性卡尔曼滤波器。一旦我们有了高斯,我们已经开发出来的所有数学工具都会发挥作用!

Unscented变换的准确性

MerweScaledSigmaPoints() JulierSigmaPoints()这些函数,都可以产生5个西格玛点,以及相应的Wm, Wc。

unscented_transform() 可以将西格玛点变换后的结果,和Wm Wc混合在一起,生成结果的均值和协方差矩阵。

结果非常棒!用500000个点模拟出来的效果,和用5个西格玛点模拟出来的效果几乎一致。我对此的理解是:

从一个分布中,找出5个点,给予不同的权值;然后对这5个点执行非线性变换,把5个结果,用先前的权值组合起来,居然均值和协方差,和总样本相差无几!

所以,找出哪5个点,给予什么样的权重,就是无迹变换的核心了。

Van der Merwe的缩放西格玛点算法

Rudolph Van der Merwe在2004发明的方法,它在各种问题中表现良好,并且在性能和准确性之间有良好的折衷。

该公式使用3个参数来控制西格玛点如何分布和加权 α , β, and κ.

西格玛点的数目为2n+1个。在二维情况下,就是5个西格玛点。第一个是均值,其他4个按一定公式得到,对称分布在四周。

11-扩展卡尔曼滤波

12-粒子过滤器

适合粒子过滤器的实际情况是:

multimodal:我们想要同时跟踪零个,一个或多个对象。

遮挡:一个对象可以隐藏另一个对象,从而导致对多个对象进行一次测量。

非线性行为:飞机受到风的冲击,球以抛物线运动,人们相互碰撞。

非线性测量:雷达给我们到物体的距离。将它转换为(x,y,z)坐标需要非线性的平方根。

非高斯噪声:当物体在背景中移动时,计算机视觉会将部分背景误认为物体。

连续的:物体的位置和速度(即状态空间)可以平滑地随时间变化。

多变量:我们想追踪几个属性,比如位置,速度,转弯率等。

未知的过程模型:我们可能不知道系统的过程模型

我们所学的过滤器都不能很好地处理所有这些限制。

离散贝叶斯过滤器:这具有大部分属性。它是多模式的,可以处理非线性测量,并且可以扩展以处理非线性行为。但是,它是离散的和单变量的。

卡尔曼滤波器:卡尔曼滤波器针对具有高斯噪声的单峰线性系统产生最优估计。这些都不适用于我们的问题。

Unscented卡尔曼滤波器:UKF处理非线性,连续,多变量问题。但是,它不是多模式的,也不处理遮挡。它可以处理适度非高斯的噪声,但对于非高斯分布或非线性问题并不适用。

扩展卡尔曼滤波器:EKF具有与UKF相同的优势和局限性,但它对强非线性和非高斯噪声更为敏感。

通用粒子滤波算法

1. 随机生成一堆粒子

粒子可以拥有你需要估计的位置,航向和/或任何其他状态变量。每个人都有一个权重(概率),表明它与系统的实际状态匹配的可能性。用相同的重量初始化每一个。

2.预测粒子的下一个状态

根据预测真实系统的行为方式移动粒子。

3.更新

根据测量值更新粒子的权重。与测量非常匹配的粒子的权重要高于与测量结果不匹配的粒子的权重。

4.重新取样

丢弃非常不可能的颗粒并用更可能的颗粒的副本代替它们。

5.计算估算

可选地,计算该组粒子的加权均值和协方差以得到状态估计。

13-平滑

建议始终使用RTS平滑

14-自适应滤波

一个例子,车辆匀速直线运动,但在一个特定的位置进行了加速度转向。现在有3种滤波方案:

1)不考虑加速度的滤波方案。这种方案在转向后飞了,很久才能回到正确的轨迹。

2)上述方案,把Q加大。这种方案更相信测量值,所以转完后很快就能跟上轨迹,但平时受测量误差影响,不够平滑。

3)采用考虑加速度的滤波方案。这种方案在转向后立即跟上,但是在稳态行为期间以非常嘈杂的输出为代价。

看来我们赢不了。恒速滤波器在目标加速时不能快速反应,但恒定的加速滤波器在零加速状态期间将噪声误解为加速而不是噪音。

检测机动

所谓机动发生的时候,就是物体运动模型和预测模型差别产生的时候,此时,残差最大。

可调过程噪音

我们将考虑的第一种方法将使用低阶模型,并根据是否正在进行机动动作来调整过程噪音。当残差变得“很大”(对于大的合理定义),我们会增加过程噪音。这将导致过滤器偏好过程预测的测量结果,并且过滤器将严密跟踪信号。当残差很小时,我们将缩小过程噪音。

连续调整

第一种方法(来自Bar-Shalom [1])使用以下等式对残差的平方进行归一化。

残差一旦超过某个极限(例如4个标准差),我们将放大Q(例如乘以1000倍)。如果残差又低于该极限,则恢复Q。

该滤波器的性能明显好于等速滤波器。操纵开始后,等速滤波器花费大约10秒时间重新获取信号。自适应滤波器在一秒钟内完成相同的操作。

连续调整 - 标准偏差版本

另一种非常类似的方法来自ZrAgman〔2〕,基于测量误差协方差的标准偏差设置极限。

如果残差的绝对值大于上述计算的标准偏差的某个倍数,我们将过程噪声增加一个固定的量,重新计算Q,并继续。

衰落记忆过滤器

如果目标是机动的,它并不总是像过程模型预测的那样工作。在这种情况下,记住所有过去的测量和估算是一种负担。

衰落记忆过滤器通过减少旧的测量权重来解决这个问题,并且对于最近的测量更重要。

方法是在P的变换过程中,增加一个乘法因子a。如果a=1,则就是普通卡尔曼滤波。一般a比1大一点点,例如1.01

用于上述“车辆匀速直线运动,但在一个特定的位置进行了加速度转向”的例子,a=1.02效果好了一些,a=1.05效果更好了。

附录A 安装

这本书需要IPython,Jupyter,NumPy,SciPy,SymPy和Matplotlib。这些都是开源的。

我已经在Ubuntu中安装了NumPy,SciPy,matplotlib。IPython,Jupyter如果不用来做交互式网页看书的话,不用安装。SymPy是用于执行符号数学的Python包,尚未安装。

安装 FilterPy(作者的成果)

sudo pip install filterpy

编辑于 2018-05-17