【发布时间】:2018-08-10 03:55:51
【问题描述】:
所以,我遇到了这个错误:
temp7:
1.68219771725e-06
temp6:
[[array([-60800056.33037408], dtype=object)]]
Exception in thread Thread-4:
Traceback (most recent call last):
File "/usr/lib/python2.7/threading.py", line 810, in __bootstrap_inner
self.run()
File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/timer.py", line 223, in run
self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
File "/home/husky/catkin_ws/src/huskybot_ekf_v2/src/meas_update.py", line 194, in meas_update_step
thisJ = temp7 * math.exp(temp6)
似乎在最后一行thisJ =temp7* math.exp(temp6),我不能取temp6 的指数。有谁知道原因吗?
for i in range(0,len(line_features)-1):
for k in range(0, len(m)):
mkx = m[k][0]
mky = m[k][1]
posex = predicted_pose.x
posey = predicted_pose.y
poseth = predicted_pose.theta
distx = math.pow((mkx - posex), 2);
disty = math.pow((mky - posey), 2);
q = distx + disty
angle = math.atan2(mky-posey, mkx-posex) - poseth
expected_meas = numpy.array([[math.sqrt(q)],
[angle],
[0]
])
H = numpy.array([[-(mkx-posex)/math.sqrt(q), -(mky-posey)/math.sqrt(q) , 0],
[(mky-posey)/q, -(mkx-posex)/q, -1],
[0 , 0, 0]])
Qt = 0.5
innovation_cov = H*predicted_cov*numpy.transpose(H)+Qt
innovation = ave_meas_dist - expected_meas
dd = [[2]]
#dd = array[0]
#print 0.5*numpy.array(dd)
temp5 = -1/2 * numpy.dot(numpy.transpose(innovation), numpy.linalg.inv(innovation_cov))
temp6 = numpy.dot(temp5, innovation)
print math.sqrt(numpy.linalg.det(2*math.pi*innovation_cov)) * numpy.array(dd)
temp7 = math.sqrt(numpy.linalg.det(2*math.pi*innovation_cov))
thisJ =temp7* math.exp(temp6)
【问题讨论】:
-
请提供minimal reproducible example,以便我们进行测试。