So, I ran into this error:
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)
It seems like on the very last line thisJ =temp7* math.exp(temp6)
, that I can't take the exponent of temp6. Does anyone know the cause?
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)
See Question&Answers more detail:
os 与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…