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 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 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 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 predict(self, dt):
if self.first_detected:
self._update_dt(dt)
state = cv2.KalmanPredict(self.kalman)
lanes = []
for i in range(0, state.rows, 8):
lanes.append(
(state[i, 0], state[i + 2, 0], state[i + 4,
0], state[i + 6, 0]))
#print 'r:',state[i,0],state[i+2,0],state[i+4,0],state[i+6,0]
return lanes
else:
return None
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 predictInternal(self):
self.updateProcessNoiseCov()
prediction = cv.KalmanPredict(self.kf)
return tuple([int(prediction[i, 0]) for i in range(8)])
cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5))
cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1))
cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1))
cv.RandArr(rng, kalman.state_post, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(0.1))
while True:
def calc_point(angle):
return (cv.Round(img.width / 2 + img.width / 3 * cos(angle)),
cv.Round(img.height / 2 - img.width / 3 * sin(angle)))
state_angle = state[0, 0]
state_pt = calc_point(state_angle)
prediction = cv.KalmanPredict(kalman)
predict_angle = prediction[0, 0]
predict_pt = calc_point(predict_angle)
cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(sqrt(kalman.measurement_noise_cov[0, 0])))
# generate measurement
cv.MatMulAdd(kalman.measurement_matrix, state, measurement,
measurement)
measurement_angle = measurement[0, 0]
measurement_pt = calc_point(measurement_angle)
# plot points
def draw_cross(center, color, d):
def __predictKalman(self):
self.kalman_prediction = cv.KalmanPredict(self.kalman)
self.predict_pt = (self.kalman_prediction[0, 0],
self.kalman_prediction[1, 0])
def getPrediction(self):
self.predicted = cv.KalmanPredict(self.kalman)
return self.predicted[0, 0], self.predicted[1, 0]
def predict(kf):
predicted = cv.KalmanPredict(kf)
return (predicted[0, 0], predicted[1, 0], predicted[2, 0], predicted[3, 0],
predicted[4, 0], predicted[5, 0])
def predict(self):
kalman_prediction = cv.KalmanPredict(self.kalman)
predict_pt = (kalman_prediction[0, 0], kalman_prediction[1, 0])
return predict_pt
