def __init__(self, processNoiseCovariance=1e-4, measurementNoiseCovariance=1e-1, errorCovariancePost=0.1): self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) for j in range(4): for k in range(4): self.kalman.transition_matrix[j, k] = 0 self.kalman.transition_matrix[j, j] = 1 cv.SetIdentity(self.kalman.measurement_matrix) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def __setKalman(self): ts = self if len(ts) < 2: self.kalman_x = ts[-1].x self.kalman_y = ts[-1].y else: self.kalman_x = ts[-2].x self.kalman_y = ts[-2].y self.kalman.state_pre[0, 0] = self.kalman_x self.kalman.state_pre[1, 0] = self.kalman_y self.kalman.state_pre[2, 0] = self.predict_pt[0] self.kalman.state_pre[3, 0] = self.predict_pt[1] self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[0, 1] = 0 self.kalman.transition_matrix[0, 2] = 0 self.kalman.transition_matrix[0, 3] = 0 self.kalman.transition_matrix[1, 0] = 0 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[1, 2] = 0 self.kalman.transition_matrix[1, 3] = 0 self.kalman.transition_matrix[2, 0] = 0 self.kalman.transition_matrix[2, 1] = 0 self.kalman.transition_matrix[2, 2] = 0 self.kalman.transition_matrix[2, 3] = 1 self.kalman.transition_matrix[3, 0] = 0 self.kalman.transition_matrix[3, 1] = 0 self.kalman.transition_matrix[3, 2] = 0 self.kalman.transition_matrix[3, 3] = 1 cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1))
def __init__(self, useProgressivePNC=False, pnc=1e-4): # state vector: # 0-7: x, y for each corner # 8-16: derivatives of 0-7 respectively self.useProgressivePNC = useProgressivePNC self.kf = cv.CreateKalman(16, 8, 0) self.setOldCvMat(self.kf.transition_matrix, [ [1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], ]) self.measurement = cv.CreateMat(8, 1, cv.CV_32FC1) cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(pnc)) cv.SetIdentity(self.kf.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kf.error_cov_post, cv.RealScalar(1))
def __init__(self): self.tfListen = tf.TransformListener() rospy.sleep(1.0) self.kalman = cv.CreateKalman(3, 3, 0) self.kalman_state = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(3, 1, cv.CV_32FC1) self.kalman.state_pre[0, 0] = 0 #first x self.kalman.state_pre[1, 0] = 0 #first y self.kalman.state_pre[2, 0] = 0 #first theta # set kalman transition matrix self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[2, 2] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1)) self.get_hive = rospy.Service('get_hive', GetLocation, self.get_hive) rospy.Subscriber('large_markers', AlvarMarkers, self.cb_ar_marker)
def __init__(self, processNoiseCovariance=1e-1, measurementNoiseCovariance=1e-1, errorCovariancePost=20): self.stateNum = 36 self.measureNum = 18 self.kalman = cv.CreateKalman(self.stateNum, self.measureNum, 0) self.kalman_state = cv.CreateMat(self.stateNum, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(self.stateNum, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(self.measureNum, 1, cv.CV_32FC1) for i in range(self.stateNum): for j in range(self.stateNum): if i == j or (j - self.measureNum) == i: self.kalman.transition_matrix[i, j] = 1.0 else: self.kalman.transition_matrix[i, j] = 0.0 cv.SetIdentity(self.kalman.measurement_matrix) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def __init__(self, useProgressivePNC=False, pnc=1e-3): # state vector: # 0: min_x # 1: min_y # 2: max_x # 3: max_y # 4-7: derivatives of 0-4 respectively # measurement vector: # 0-4: same as state (no derivatives) self.useProgressivePNC = useProgressivePNC self.kf = cv.CreateKalman(8, 4, 0) self.setOldCvMat(self.kf.transition_matrix, [ [1, 0, 0, 0, 1, 0, 0, 0], [0, 1, 0, 0, 0, 1, 0, 0], [0, 0, 1, 0, 0, 0, 1, 0], [0, 0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 1], ]) self.measurement = cv.CreateMat(4, 1, cv.CV_32FC1) cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(pnc)) cv.SetIdentity(self.kf.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kf.error_cov_post, cv.RealScalar(1))
def __init__(self, processNoiseCovariance=1e-4, measurementNoiseCovariance=1e-1, errorCovariancePost=0.1): ''' Constructs a new Kalman2D object. For explanation of the error covariances see http://en.wikipedia.org/wiki/Kalman_filter ''' # 状态空间:位置--2d,速度--2d self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) for j in range(4): for k in range(4): self.kalman.transition_matrix[j,k] = 0 self.kalman.transition_matrix[j,j] = 1 #加入速度 x = x + vx, y = y + vy # 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1 #如果把下面两句注释掉,那么位置跟踪kalman滤波器的状态模型就是没有使用速度信息 # self.kalman.transition_matrix[0, 2]=1 # self.kalman.transition_matrix[1, 3]=1 cv.SetIdentity(self.kalman.measurement_matrix) #初始化带尺度的单位矩阵 cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(processNoiseCovariance)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(measurementNoiseCovariance)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(errorCovariancePost)) self.predicted = None self.esitmated = None
def from_message(self, msgs, alpha=0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ self.size = (msgs[0].width, msgs[0].height) self.T = cv.CreateMat(3, 1, cv.CV_64FC1) self.R = cv.CreateMat(3, 3, cv.CV_64FC1) cv.SetIdentity(self.T) cv.SetIdentity(self.R) self.l.from_message(msgs[0]) self.r.from_message(msgs[1]) # Need to compute self.T and self.R here, using the monocular parameters above if False: self.set_alpha(0.0)
def __init__(self, pt): self.pt = pt self.history = [pt] self.mse = 0.0 self.ns = 1 self.lifespan = 1 self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1) # set previous state for prediction self.kalman.state_pre[0, 0] = pt[0] self.kalman.state_pre[1, 0] = pt[1] self.kalman.state_pre[2, 0] = 0 self.kalman.state_pre[3, 0] = 0 # set kalman transition matrix self.kalman.transition_matrix[0, 0] = 1 self.kalman.transition_matrix[0, 1] = 0 self.kalman.transition_matrix[0, 2] = 0 self.kalman.transition_matrix[0, 3] = 0 self.kalman.transition_matrix[1, 0] = 0 self.kalman.transition_matrix[1, 1] = 1 self.kalman.transition_matrix[1, 2] = 0 self.kalman.transition_matrix[1, 3] = 0 self.kalman.transition_matrix[2, 0] = 0 self.kalman.transition_matrix[2, 1] = 0 self.kalman.transition_matrix[2, 2] = 0 self.kalman.transition_matrix[2, 3] = 1 self.kalman.transition_matrix[3, 0] = 0 self.kalman.transition_matrix[3, 1] = 0 self.kalman.transition_matrix[3, 2] = 0 self.kalman.transition_matrix[3, 3] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1)) self.assignObs(pt, 3)
def init_kalman(self): self.kalman = cv.CreateKalman(3,3,0) self.kalman_state = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman_process_noise = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(3,1, cv.CV_32FC1) self.kalman.state_pre[0,0] = 0 #first x self.kalman.state_pre[1,0] = 0 #first y self.kalman.state_pre[2,0] = 0 #first theta # set kalman transition matrix self.kalman.transition_matrix[0,0] = 1 self.kalman.transition_matrix[1,1] = 1 self.kalman.transition_matrix[2,2] = 1 # set Kalman Filter cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(self.kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(1))
def init_kalman(kalman, x, y, z): kalman.transition_matrix[:, :] = 0 for i in range(6): kalman.transition_matrix[i, i] = 1 kalman.transition_matrix[0, 3] = dT kalman.transition_matrix[1, 4] = dT kalman.transition_matrix[2, 5] = dT kalman.state_post[:, :] = 0 kalman.control_matrix[:, :] = 0 kalman.control_matrix[5, 0] = 1 cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5)) cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) kalman.state_post[0, 0] = x kalman.state_post[1, 0] = y kalman.state_post[2, 0] = z
def cal_fromcorners(self, good): # Perform monocular calibrations lcorners = [(l, b) for (l, r, b) in good] rcorners = [(r, b) for (l, r, b) in good] self.l.cal_fromcorners(lcorners) self.r.cal_fromcorners(rcorners) lipts = self.mk_image_points(lcorners) ripts = self.mk_image_points(rcorners) boards = [b for (_, _, b) in good] opts = self.mk_object_points(boards, True) npts = self.mk_point_counts(boards) flags = cv2.CALIB_FIX_INTRINSIC self.T = cv.CreateMat(3, 1, cv.CV_64FC1) self.R = cv.CreateMat(3, 3, cv.CV_64FC1) cv.SetIdentity(self.T) cv.SetIdentity(self.R) cv.StereoCalibrate( opts, lipts, ripts, npts, self.l.intrinsics, self.l.distortion, self.r.intrinsics, self.r.distortion, self.size, self.R, # R self.T, # T cv.CreateMat(3, 3, cv.CV_32FC1), # E cv.CreateMat(3, 3, cv.CV_32FC1), # F (cv.CV_TERMCRIT_ITER + cv.CV_TERMCRIT_EPS, 1, 1e-5), flags) self.set_alpha(0.0)
def init_kalman(self, pose): kalman = cv.CreateKalman(6, 3, 0) #kalman_state = cv.CreateMat(6,1, cv.CV_32FC1) #kalman_process_noise = cv.CreateMat(3,1, cv.CV_32FC1) quat = quat_msg_to_array(pose.pose.orientation) r, p, theta = tf.transformations.euler_from_quaternion(quat) kalman.state_post[0, 0] = pose.pose.position.x #first x kalman.state_post[1, 0] = pose.pose.position.y #first y kalman.state_post[2, 0] = theta #first theta kalman.state_post[3, 0] = 0 #delta x kalman.state_post[4, 0] = 0 #delta y kalman.state_post[5, 0] = 0 #delta theta # set kalman transition matrix kalman.transition_matrix[0, 0] = 1 #kalman.transition_matrix[0,3] = 1 kalman.transition_matrix[1, 1] = 1 #kalman.transition_matrix[1,4] = 1 kalman.transition_matrix[2, 2] = 1 #kalman.transition_matrix[2,5] = 1 kalman.transition_matrix[3, 3] = 1 kalman.transition_matrix[4, 4] = 1 kalman.transition_matrix[5, 5] = 1 # set Kalman Filter cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-3)) cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1)) cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1)) return kalman
def updateProcessNoiseCov(self): if not self.useProgressivePNC: return self.timestep += 1 startTS = 0 endTS = 400 startCov = 1e-4 endCov = 1e-3 covSlope = (endCov - startCov) / (endTS - startTS) covIntercept = startCov cov = covSlope * (self.timestep - 30) + covIntercept print "frame %d: cov=%.4g" % (self.timestep, cov) cv.SetIdentity(self.kf.process_noise_cov, cv.RealScalar(cov))
def cal_fromcorners(self, good): """ :param good: Good corner positions and boards :type good: [(corners, ChessboardInfo)] """ boards = [b for (_, b) in good] ipts = self.mk_image_points(good) opts = self.mk_object_points(boards) npts = self.mk_point_counts(boards) intrinsics = cv.CreateMat(3, 3, cv.CV_64FC1) if self.calib_flags & cv2.CALIB_RATIONAL_MODEL: distortion = cv.CreateMat(8, 1, cv.CV_64FC1) # rational polynomial else: distortion = cv.CreateMat(5, 1, cv.CV_64FC1) # plumb bob cv.SetZero(intrinsics) cv.SetZero(distortion) # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio intrinsics[0, 0] = 1.0 intrinsics[1, 1] = 1.0 cv.CalibrateCamera2(opts, ipts, npts, self.size, intrinsics, distortion, cv.CreateMat(len(good), 3, cv.CV_32FC1), cv.CreateMat(len(good), 3, cv.CV_32FC1), flags=self.calib_flags) self.intrinsics = intrinsics self.distortion = distortion # R is identity matrix for monocular calibration self.R = cv.CreateMat(3, 3, cv.CV_64FC1) cv.SetIdentity(self.R) self.P = cv.CreateMat(3, 4, cv.CV_64FC1) cv.SetZero(self.P) self.mapx = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1) self.mapy = cv.CreateImage(self.size, cv.IPL_DEPTH_32F, 1) self.set_alpha(0.0)
def KalmanFilter(): kf = cv.CreateKalman(6, 2, 0) ''' init the prediction/evolution/transition matrix | 1 0 1 0 .5 0 | x | | x + vx + .5ax | | 0 1 0 1 0 .5 | y | = | y + vy + .5ay | | 0 0 1 0 1 0 | vx | | vx + ax | | 0 0 0 1 0 1 | vy | | vy + ay | | 0 0 k 0 0 0 | ax | | k*vx | | 0 0 0 0 0 1 | ay | | ay | ''' # diagonals for j in range(6): for k in range(6): kf.transition_matrix[j, k] = 0 kf.transition_matrix[j, j] = 1 # x + vx + 0.5ax kf.transition_matrix[0, 2] = 1 kf.transition_matrix[0, 4] = 0.5 # y + vy + 0.5ay kf.transition_matrix[1, 3] = 1 kf.transition_matrix[1, 5] = 0.5 # vx + ax kf.transition_matrix[2, 4] = 1 # vy + ay kf.transition_matrix[3, 5] = 1 # predict ax = k * vx kf.transition_matrix[4, 4] = 0 kf.transition_matrix[4, 2] = horizontal_acc_const ''' measurement matrix H: mean = H * state | 1 0 | x | | x | | 0 1 | y | = | y | ''' kf.measurement_matrix[0, 0] = 1 kf.measurement_matrix[1, 1] = 1 # process noise cov matrix Q: models the EXTERNAL uncertainty cv.SetIdentity(kf.process_noise_cov, PN_Cov) # measurement noise cov matrix R: covariance of SENSOR noise cv.SetIdentity(kf.measurement_noise_cov, Sensor_Cov) ''' error estimate covariance matrix P: relates the correlation of state vars priori: before measurement posteriori: after measurement diagonals are all 1. x-vy and y-vy also correlated. | xx xy xvx xvy xax xay | | 1 0 1 0 0 0 | | yx yy yvx yvy yax yay | | 0 1 0 1 0 0 | | vxx vxy vxvx vxvy vxax vxay | = | 1 0 1 0 0 0 | | vyx vyy vyvx vyvy vyax vyay | | 0 1 0 1 0 0 | | axx axy axvx axvy axax axay | | 0 0 0 0 1 0 | | ayx ayy ayvx ayvy ayax ayay | | 0 0 0 0 0 1 | ''' cv.SetIdentity(kf.error_cov_post, 1) kf.error_cov_post[0, 2] = 1 kf.error_cov_post[1, 3] = 1 kf.error_cov_post[2, 0] = 1 kf.error_cov_post[3, 1] = 1 return kf
rng = cv.RNG(-1) code = -1L cv.Zero(measurement) cv.NamedWindow("Kalman", 1) while True: cv.RandArr(rng, state, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1)) kalman.transition_matrix[0, 0] = 1 kalman.transition_matrix[0, 1] = 1 kalman.transition_matrix[1, 0] = 0 kalman.transition_matrix[1, 1] = 1 cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) 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)