带filterpy的扩展卡尔曼滤波器

Question

我正在尝试按照https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/11-Extended-Kalman-Filters.ipynb

的示例，使用2D输入（x，y）实现filterpy库的扩展Kalman滤波器。

但是，我无法让它运行2维。我很高兴获得此函数在2D坐标系中的使用示例。

from filterpy.kalman import ExtendedKalmanFilter
from filterpy.common import Q_discrete_white_noise
import numpy as np

coordinates = [] # Fill coordinates with [x, y] pairs 
dt = 0.05 f = ExtendedKalmanFilter(2, 2)  
f.x[:2] = coordinates[0] 
f.R = np.diag([5 ** 2]) 
f.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1) 
f.Q[1, 1] = 0.1 
f.P *= 50 
for c in coordinates:
    f.predict_update(c, jacobF, Hx)
    print(f.x)

Jacobian和H矩阵计算定义如下：

def Hx(x):
    return x

def jacobF(pt):
    x_dist = pt[0]
    y_dist = pt[1]
    denom = np.sqrt(x_dist**2 + y_dist**2)
    return np.array([x_dist/denom, y_dist/denom], dtype=np.float).transpose()