x = x_initial
P = P_initial
x_result = []
time_result = []
v_result = []
for i in range(len(lidar_measurements) - 1):
# calculate time that has passed between lidar measurements
delta_t = (lidar_time[i + 1] - lidar_time[i]) / 1000.0
# Prediction Step - estimates how far the object traveled during the time interval
F = F_matrix(delta_t)
Q = Q_matrix(delta_t, acceleration_variance)
x_prime = F * x
P_prime = F * P * F.T() + Q
# Measurement Update Step - updates belief based on lidar measurement
y = m.Matrix([[lidar_measurements[i + 1]]]) - H * x_prime
S = H * P_prime * H.T() + R
K = P_prime * H.T() * S.inverse()
x = x_prime + K * y
P = (I - K * H) * P_prime
# Store distance and velocity belief and current time
x_result.append(x[0][0])
v_result.append(3600.0/1000 * x[1][0])
time_result.append(lidar_time[i+1])
result = pd.DataFrame(
{'time': time_result,
'distance': x_result,
'velocity': v_result
})
TypeError Traceback (most recent call last)
in ()
19
20 x_prime = F * x
---> 21 P_prime = F * P * F.T() + Q
22
23 # Measurement Update Step - updates belief based on lidar measurement
TypeError: unsupported operand type(s) for *: 'NoneType' and 'NoneType'
P_prime = F * P * F.T() + Q
这里的
f p q看看是不是有none