2

我正在尝试使用 pykalman 的 AdditiveUnscentedKalman 过滤器。我得到了嘈杂的 x,y 值，我试图将 x,y,xdot,ydot 从过滤器中取出。

我正在从过滤器中获取值，但这不是我所期望的。State: [[ 481.65737052 477.23904382 0. 0. ] [ 659.29999618 659.28265402 58.33365188 59.77883149]] Obs: [478, 660, -0.4666666666666667, -0.36666666666666664, -2.4756234162106834, 1.2145259141964182]

obs[0] 和 obs[1] 是测量的 xy，而 state 是从过滤器中出来的。state[0][0] 和 state[1][0] 看起来是 x,y，而 state[0][1] 和 state[1][1] 似乎是 x,y。我不知道其他数字应该是什么，因为它们是不可接受的速度。

如果有人可以验证我正在使用正确的转换功能，那将不胜感激。

transition_covariance = np.array([[100.0, 0.0, 0.0, 0.0],
                                         [0.0, 100.0, 0.0, 0.0],
                                         [0.0, 0.0, 100.0, 0.0],
                                         [0.0, 0.0, 0.0, 100.0]])

observation_covariance = np.array([[0.4, 0.0],
                                       [0.0, 0.4]])

initial_state_covariance = np.array([[100.0, 0.0, 0.0, 0.0],
                                         [0.0, 100.0, 0.0, 0.0],
                                         [0.0, 0.0, 1000.0, 0.0],
                                         [0.0, 0.0, 0.0, 1000.0]])

self.ukf = AdditiveUnscentedKalmanFilter(transition_functions = self.transition_function,
                                             observation_functions = self.observation_function,
                                             transition_covariance = transition_covariance,
                                             observation_covariance = observation_covariance,
                                             initial_state_mean = initial_conditions,
                                             initial_state_covariance = initial_state_covariance)

def get_states(self, observations):
    return self.ukf.filter(observations)

# [x, y, xvel, yvel]
def transition_function(self, state):
    return np.array([state[0] + state[2] * self.dt,
                     state[1] + state[3] * self.dt,
                     state[2],
                     state[3]])

def observation_function(self, state):
    om = np.array([[1.0, 0.0, 0.0, 0.0],
                   [0.0, 1.0, 0.0, 0.0]])
    return np.matmul(om, state)

我很困惑，因为 .filter 调用的输出是一个 2X4 的矩阵，我希望它是一个 1X4。

4

0 回答 0