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])