def predict_pose(self, pose):
quat = quat_msg_to_array(pose.pose.orientation)
r,p,theta = tf.transformations.euler_from_quaternion(quat)
if self.kalman.state_post[0,0] == 0 and self.kalman.state_post[1,0] == 0:
self.kalman.state_post[0,0] = pose.pose.position.x
self.kalman.state_post[1,0] = pose.pose.position.y
self.kalman.state_post[2,0] = theta
self.kalman.state_post[2,0] = self.wrap_ang(self.kalman.state_post[2,0])
theta = self.wrap_theta(self.kalman.state_post[2,0], theta)
kalman_prediction = cv.KalmanPredict(self.kalman)
self.kalman_measurement[0,0] = pose.pose.position.x
self.kalman_measurement[1,0] = pose.pose.position.y
self.kalman_measurement[2,0] = theta
kalman_estimated = cv.KalmanCorrect(self.kalman, self.kalman_measurement)
point = (kalman_estimated[0,0], kalman_estimated[1,0], kalman_estimated[2,0])
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
q = tf.transformations.quaternion_from_euler(0, 0, point[2])
pose.pose.orientation = Quaternion(*q)
pose.header.stamp = rospy.Time.now()
return pose
def predict_pose(self, kalman, pose):
quat = quat_msg_to_array(pose.pose.orientation)
r, p, theta = tf.transformations.euler_from_quaternion(quat)
kalman.state_post[2, 0] = self.wrap_ang(kalman.state_post[2, 0])
theta = self.wrap_theta(kalman.state_post[2, 0], theta)
kalman_prediction = cv.KalmanPredict(kalman)
kalman_measurement = cv.CreateMat(3, 1, cv.CV_32FC1)
kalman_measurement[0, 0] = pose.pose.position.x
kalman_measurement[1, 0] = pose.pose.position.y
kalman_measurement[2, 0] = theta
kalman_estimated = cv.KalmanCorrect(kalman, kalman_measurement)
point = (kalman_estimated[0,
0], kalman_estimated[1,
0], kalman_estimated[2,
0])
pose.pose.position.x = point[0]
pose.pose.position.y = point[1]
q = tf.transformations.quaternion_from_euler(0, 0, point[2])
pose.pose.orientation = Quaternion(*q)
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = odom
return pose
def correct(kf, x, y):
global measurement
measurement[0, 0] = x
measurement[1, 0] = y
corrected = cv.KalmanCorrect(kf, measurement)
return (corrected[0, 0], corrected[1, 0], corrected[2, 0], corrected[3, 0],
corrected[4, 0], corrected[5, 0])
def update(self, pre):
pre = pre.reshape(1, -1)
for k in range(self.measureNum):
self.kalman_measurement[k, 0] = pre[0, k]
self.predicted = cv.KalmanPredict(self.kalman)
self.corrected = cv.KalmanCorrect(self.kalman, self.kalman_measurement)
def observeInternal(self, measurement):
measurementMatrix = map(lambda v: [v], measurement)
if not self.inited:
self.inited = True
self.setOldCvMat(self.kf.state_pre, measurementMatrix)
else:
self.setOldCvMat(self.measurement, measurementMatrix)
cv.KalmanCorrect(self.kf, self.measurement)
def update(self, x, y):
'''
Updates the filter with a new X,Y measurement
'''
self.kalman_measurement[0, 0] = x
self.kalman_measurement[1, 0] = y
self.predicted = cv.KalmanPredict(self.kalman)
self.corrected = cv.KalmanCorrect(self.kalman, self.kalman_measurement)
def update(self, lanes):
if self.first_detected:
for l, i in zip(lanes, range(0, self.meas_size, 4)):
if l is not None:
c = 0
for t in range(i, i + 4):
self.meas[t, 0] = l[c]
c += 1
cv2.KalmanCorrect(self.kalman, self.meas)
else:
if lanes.count(None) == 0:
print "============"
self._first_detect(lanes)
def assignObs(self, pt, err):
#update the kalman
self.kalman_measurement[0, 0] = float(pt[0])
self.kalman_measurement[1, 0] = float(pt[1])
kalman_estimated = cv.KalmanCorrect(self.kalman,
self.kalman_measurement)
state_pt = (kalman_estimated[0, 0], kalman_estimated[1, 0])
#print "State estimate: ", state_pt
print "Adding pt, ", pt, " to history: ", self.history
#self.kalman.correct(pt)
self.history.append(pt)
self.mse = self.mse + err * err
self.ns = self.ns + 1
def track(kalman, x, y, z):
vx = kalman.state_post[3, 0]
vy = kalman.state_post[4, 0]
vz = kalman.state_post[5, 0]
v = math.sqrt(vx**2 + vy**2 + vz**2)
kalman.transition_matrix[3, 3] = 1 - kD * v
kalman.transition_matrix[4, 4] = 1 - kD * v
kalman.transition_matrix[5, 5] = 1 - kD * v
control[0, 0] = -GRAVITY * dT
cv.KalmanPredict(kalman, control)
pred = [
kalman.state_pre[0, 0], kalman.state_pre[1, 0], kalman.state_pre[2, 0]
]
cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(0.005))
measurement[0, 0] += x
measurement[1, 0] += y
measurement[2, 0] += z
cv.KalmanCorrect(kalman, measurement)
meas = [measurement[0, 0], measurement[1, 0], measurement[2, 0]]
return pred, meas, np.array([x, y, z])
def draw_cross(center, color, d):
cv.Line(img, (center[0] - d, center[1] - d),
(center[0] + d, center[1] + d), color, 1, cv.CV_AA, 0)
cv.Line(img, (center[0] + d, center[1] - d),
(center[0] - d, center[1] + d), color, 1, cv.CV_AA, 0)
cv.Zero(img)
draw_cross(state_pt, cv.CV_RGB(255, 255, 255), 3)
draw_cross(measurement_pt, cv.CV_RGB(255, 0, 0), 3)
draw_cross(predict_pt, cv.CV_RGB(0, 255, 0), 3)
cv.Line(img, state_pt, measurement_pt, cv.CV_RGB(255, 0, 0), 3,
cv.CV_AA, 0)
cv.Line(img, state_pt, predict_pt, cv.CV_RGB(255, 255, 0), 3,
cv.CV_AA, 0)
cv.KalmanCorrect(kalman, measurement)
cv.RandArr(rng, process_noise, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(sqrt(kalman.process_noise_cov[0, 0])))
cv.MatMulAdd(kalman.transition_matrix, state, process_noise, state)
cv.ShowImage("Kalman", img)
code = cv.WaitKey(100) % 0x100
if code != -1:
break
if code in [27, ord('q'), ord('Q')]:
break
cv.DestroyWindow("Kalman")
# set Kalman Filter
cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1))
cv.SetIdentity(kalman.process_noise_cov,
cv.RealScalar(1e-5)) ## 1e-5
cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1))
cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(0.1))
else:
# Kalman prediction with Kalman Correction with the points I have in trajectory_0000.txt
kalman_prediction = cv.KalmanPredict(kalman)
rightPoints = cv.CreateMat(2, 1, cv.CV_32FC1)
rightPoints[0, 0] = x
rightPoints[1, 0] = y
kalman.state_pre[0, 0] = x
kalman.state_pre[1, 0] = y
kalman.state_pre[2, 0] = 0
kalman.state_pre[3, 0] = 0
estimated = cv.KalmanCorrect(kalman, rightPoints)
i = i + 1
print str(x) + " - " + str(y)
# Here we do not have more points to apply the Kalman Correct, so I need to predict the points
for i in range(20):
kalman_prediction = cv.KalmanPredict(kalman)
x = kalman_prediction[0, 0]
y = kalman_prediction[1, 0]
print "Kalman prediction " + str(i) + ": " + str(x) + ", " + str(y)
def __correctKalman(self):
self.kalman_estimated = cv.KalmanCorrect(self.kalman,
self.kalman_measurement)
self.state_pt = (self.kalman_estimated[0, 0], self.kalman_estimated[1,
0])
