卡尔曼滤波器¶
卡尔曼滤波器是用于动态系统状态估计的线性模型 [1]。它们在许多机器人和跟踪/预测应用中已成为事实上的标准，因为它们非常适合具有可观测动态过程不确定性的系统。它们使用“观察、预测、校正”范式从原本嘈杂的信号中提取信息。在 Pyro 中，我们可以使用 pyro.contrib.tracking 库 构建带有可学习参数的可微分卡尔曼滤波器。
动态过程¶
首先，考虑这个简单的运动模型
其中 \(k\) 是状态，\(X\) 是信号估计，\(Z_k\) 是时间步长 \(k\) 时的观测值，\(\mathbf{W}_k\) 和 \(\mathbf{V}_k\) 是独立的噪声过程（即对于所有 \(j, k\)，\(\mathbb{E}[w_k v_j^T] = 0\)），我们将它们近似为高斯分布。注意状态转移是线性的。
卡尔曼更新¶
在每个时间步长，我们对均值和协方差进行预测
并对测量值进行校正
其中 \(X\) 是位置估计，\(P\) 是协方差矩阵，\(K\) 是卡尔曼增益，\(Q\) 和 \(R\) 是协方差矩阵。
有关深入推导，请参见 [2]
非线性估计：扩展卡尔曼滤波器¶
如果我们的系统是非线性的怎么办，例如 GPS 导航？考虑以下非线性系统
注意 \(\mathbf{f}\) 和 \(\mathbf{h}\) 现在是（光滑的）非线性函数。
扩展卡尔曼滤波器（EKF）通过对卡尔曼滤波器进行局部线性化（通过 泰勒级数展开）来解决这个问题。
其中 \(\mathbf{H}_k\) 是时间 \(k\) 处的雅可比矩阵，\(x_k^R\) 是先前的最优估计，并且我们忽略高阶项。在每个时间步长，我们计算基于先前预测的雅可比矩阵（此计算由 Pyro 在后台处理），并使用结果进行预测和更新。
省略推导过程，上述预测的修改现在如下
而更新现在如下
在 Pyro 中，我们只需要创建一个 EKFState 对象并使用其 predict 和 update 方法。Pyro 将执行精确推断来计算创新（innovations），我们将使用 SVI 来学习位置和测量协方差的 MAP 估计。
举个例子，我们来看一个在离散时间空间中以接近恒定速度在二维空间中移动了 100 个时间步长的物体。
[ ]:
import os
import math
import torch
import pyro
import pyro.distributions as dist
from pyro.infer.autoguide import AutoDelta
from pyro.optim import Adam
from pyro.infer import SVI, Trace_ELBO, config_enumerate
from pyro.contrib.tracking.extended_kalman_filter import EKFState
from pyro.contrib.tracking.distributions import EKFDistribution
from pyro.contrib.tracking.dynamic_models import NcvContinuous
from pyro.contrib.tracking.measurements import PositionMeasurement
smoke_test = ('CI' in os.environ)
assert pyro.__version__.startswith('1.9.1')
[ ]:
dt = 1e-2
num_frames = 10
dim = 4
# Continuous model
ncv = NcvContinuous(dim, 2.0)
# Truth trajectory
xs_truth = torch.zeros(num_frames, dim)
# initial direction
theta0_truth = 0.0
# initial state
with torch.no_grad():
xs_truth[0, :] = torch.tensor([0.0, 0.0, math.cos(theta0_truth), math.sin(theta0_truth)])
for frame_num in range(1, num_frames):
# sample independent process noise
dx = pyro.sample('process_noise_{}'.format(frame_num), ncv.process_noise_dist(dt))
xs_truth[frame_num, :] = ncv(xs_truth[frame_num-1, :], dt=dt) + dx
接下来，我们指定测量值。注意我们只测量粒子的位置。
[ ]:
# Measurements
measurements = []
mean = torch.zeros(2)
# no correlations
cov = 1e-5 * torch.eye(2)
with torch.no_grad():
# sample independent measurement noise
dzs = pyro.sample('dzs', dist.MultivariateNormal(mean, cov).expand((num_frames,)))
# compute measurement means
zs = xs_truth[:, :2] + dzs
我们将使用 Delta 自动导引 来学习位置和测量协方差的 MAP 估计。EKFDistribution 根据一系列测量值的张量计算所有 EKF 状态的联合对数密度。
[ ]:
def model(data):
# a HalfNormal can be used here as well
R = pyro.sample('pv_cov', dist.HalfCauchy(2e-6)) * torch.eye(4)
Q = pyro.sample('measurement_cov', dist.HalfCauchy(1e-6)) * torch.eye(2)
# observe the measurements
pyro.sample('track_{}'.format(i), EKFDistribution(xs_truth[0], R, ncv,
Q, time_steps=num_frames),
obs=data)
guide = AutoDelta(model) # MAP estimation
[ ]:
optim = pyro.optim.Adam({'lr': 2e-2})
svi = SVI(model, guide, optim, loss=Trace_ELBO(retain_graph=True))
pyro.set_rng_seed(0)
pyro.clear_param_store()
for i in range(250 if not smoke_test else 2):
loss = svi.step(zs)
if not i % 10:
print('loss: ', loss)
[ ]:
# retrieve states for visualization
R = guide()['pv_cov'] * torch.eye(4)
Q = guide()['measurement_cov'] * torch.eye(2)
ekf_dist = EKFDistribution(xs_truth[0], R, ncv, Q, time_steps=num_frames)
states= ekf_dist.filter_states(zs)
|
参考文献¶
[1] Kalman, R. E. 一种新的线性滤波和预测问题方法. 1960
[2] Welch, Greg 和 Bishop, Gary. 卡尔曼滤波器简介. 2006。