没有合适的资源?快使用搜索试试~ 我知道了~
一文看懂无迹卡尔曼滤波
资源推荐
资源详情
资源评论
无迹卡尔曼滤波
The Unscentd Kalman Filter
上一章我们讨论了非线性系统带来的困难。这种非线性可能出现在两个地方。它可以存在于我们的测量
中,例如测量物体的倾斜距离的雷达。使用Slant range计算x、y坐标需要取平方根:
x
=
√
slant
2
−
altitude
2
非线性也可以发生在过程模型中——我们可能正在跟踪一个在空气中旅行的球,在那里空气阻力的影响
导致非线性行为。对于这类问题,标准卡尔曼滤波器表现不佳或者根本不好
在上一章中,我展示了这样一个图。我稍微改变了一下方程,以强调非线性的影响。
#format the book
import book_format
book_format.set_style()
from kf_book.book_plots import set_figsize, figsize
import matplotlib.pyplot as plt
from kf_book.nonlinear_plots import plot_nonlinear_func
from numpy.random import normal
import numpy as np
# create 500,000 samples with mean 0, std 1
gaussian = (0., 1.)
data = normal(loc=gaussian[0], scale=gaussian[1], size=500000)
def f(x):
return (np.cos(4*(x/2 + 0.7))) - 1.3*x
plot_nonlinear_func(data, f)
我通过从输入中获取500,000个样本,通过非线性变换,并构建结果的直方图来生成它。我们称这些点
为*点。从输出直方图中,我们可以计算平均值和标准差,这将给我们一个更新的,尽管是近似的高斯
分布。
让我给你们看一下数据经过f(x)前后的散点图。
数据本身看起来是高斯分布,事实也确实如此。我的意思是它看起来像散布在平均零点周围的白噪声。
N = 30000
plt.subplot(121)
plt.scatter(data[:N], range(N), alpha=.2, s=1)
plt.title('Input')
plt.subplot(122)
plt.title('Output')
plt.scatter(f(data[:N]), range(N), alpha=.2, s=1);
相反,
g(data)
有一个明确的结构。有两个波段,中间有大量的点。在带的外面有分散的点,但在负半
轴的一边有更多的点。
你方也许已经想到,这种抽样过程可以解决我们的问题。假设对于每次更新,我们生成500,000个点,
将它们传递给函数,然后计算结果的均值和方差。这被称为蒙特卡罗方法,它被一些卡尔曼滤波器设计
所使用,比如集合滤波器和粒子滤波器。抽样不需要专业知识,也不需要封闭形式的解决方案。不管这
个函数有多非线性或表现有多差,只要我们用足够的sigma点进行采样,我们就能建立一个准确的输出
分布。
“足够的分数”是个难题。上面的图表是用50万个sigma点创建的,输出仍然不平滑。更糟糕的是,这只
适用于一维。所需点的数量随着维度数量的增加而增加。如果一个维度只需要500个点,那么两个维度
就需要500个点的平方,或者250,000个点,500个点的立方,或者125,000,000个点,以此类推。因
此,虽然这种方法确实有效,但它的计算成本非常高。集合滤波器和粒子滤波器使用了巧妙的技术来显
著降低这种维数,但计算负担仍然非常大。unscented卡尔曼滤波器使用sigma点,但通过使用确定性
方法选择点,大大减少了计算量。
Sigma Points - Sampling from a Distribution
让我们从二维协方差椭圆的角度来看这个问题。我选择2D只是因为它易于绘制;这可以扩展到任何维
度。假设某个任意的非线性函数,我们将从第一个协方差椭圆中取随机点,通过非线性函数,并绘制它
们的新位置。然后我们可以计算变换点的均值和协方差,并以此作为均值和概率分布的估计。
在左边,我们展示了一个椭圆,描绘了两个状态变量的1σ分布。箭头显示了几个随机采样点如何被一些
任意的非线性函数转换成一个新的分布。右边的椭圆是半透明的,表示它是这组点的均值和方差的估计
值。
我们写一个函数,它从高斯分布中随机抽取10000个点
import kf_book.ukf_internal as ukf_internal
ukf_internal.show_2d_transform()
μ
=
[ ]
, Σ =
[ ]
根据这个非线性系统:
{
该图显示了该函数发生的强非线性,以及如果我们以扩展卡尔曼滤波器的方式线性化将导致的大误差
(我们将在下一章中学习)。
A Quick Example
我很快就会进入Unscented卡尔曼滤波器(UKF)用于选择sigma点并执行计算的数学。但是让我们从一个
例子开始。
0
0
32 15
15 40
¯
x
=
x
+
y
¯
y
= 0.1
x
2
+
y
2
import numpy as np
from numpy.random import multivariate_normal
from kf_book.nonlinear_plots import plot_monte_carlo_mean
def f_nonlinear_xy(x, y):
return np.array([x + y, .1*x**2 + y*y])
mean = (0., 0.)
p = np.array([[32., 15.], [15., 40.]])
# Compute linearized mean
mean_fx = f_nonlinear_xy(*mean)
#generate random points
xs, ys = multivariate_normal(mean=mean, cov=p, size=10000).T
plot_monte_carlo_mean(xs, ys, f_nonlinear_xy, mean_fx, 'Linearized Mean');
我们将学习UKF可以使用许多不同的算法来生成sigma点。FilterPy提供了几种算法。这里有一种选择:
稍后会变得更清楚,但这个对象将为任何给定的均值和协方差生成加权sigma点。让我们来看一个例
子,其中点的大小表示它的权重:
你可以看到,我们有5个点围绕平均值(3,17)。它将比500,000个随机生成的点做得更好这件事可能看起
来很荒谬,但它真的会!
好,现在让我们实现这个过滤器。我们将在一维中实现一个标准线性滤波器;我们还没有准备好处理非
线性滤波器。过滤器的设计与我们目前所学的没有太大的不同,只有一点不同。KalmanFilter类使用矩
阵
F
来计算状态转换函数。矩阵意味着线性代数,它适用于线性问题,但不适用于非线性问题。所以,
我们用函数代替矩阵,就像我们上面做的那样。KalmanFilter类使用另一个矩阵
H
来实现测量函数,该
函数将状态转换为等效的测量。再说一遍,矩阵意味着线性,所以我们用函数代替矩阵。也许很清楚为
什么
H
被称为“度量函数”;对于线性卡尔曼滤波器,它是一个矩阵,但这只是一种快速计算线性函数的方
法。
话不多说,下面是一维跟踪问题的状态转换函数和测量函数,其中状态为
x
= [
x
˙
x
]
T
:
from filterpy.kalman import JulierSigmaPoints
sigmas = JulierSigmaPoints(n=2, kappa=1)
from kf_book.ukf_internal import plot_sigmas
plot_sigmas(sigmas, x=[3, 17], cov=[[1, .5], [.5, 3]])
def fx(x, dt):
xout = np.empty_like(x)
xout[0] = x[1] * dt + x[0]
xout[1] = x[1]
return xout
剩余50页未读,继续阅读
资源评论
wxj1yx
- 粉丝: 159
- 资源: 1
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功