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 __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, 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 __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, 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(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
the real and the measured points are connected with red line segment. (if Kalman filter works correctly, the yellow segment should be shorter than the red one). Pressing any key (except ESC) will reset the tracking with a different speed. Pressing ESC will stop the program. """ import urllib2 import cv2.cv as cv from math import cos, sin, sqrt import sys if __name__ == "__main__": A = [[1, 1], [0, 1]] img = cv.CreateImage((500, 500), 8, 3) kalman = cv.CreateKalman(2, 1, 0) state = cv.CreateMat(2, 1, cv.CV_32FC1) # (phi, delta_phi) process_noise = cv.CreateMat(2, 1, cv.CV_32FC1) measurement = cv.CreateMat(1, 1, cv.CV_32FC1) 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
import cv2.cv as cv kalman = cv.CreateKalman(4, 2, 0) i = 0 # I read the point from an .txt file with open('trajectory_0000.txt') as f: array = [] for line in f: # read rest of lines array.append([int(x) for x in line.split()]) vec = array.pop() x = vec[0] y = vec[1] # I obtain the (x,y) points if i == 0: # This happens only one time to initialize the kalman Filter with the first (x,y) point kalman.state_pre[0, 0] = x kalman.state_pre[1, 0] = y kalman.state_pre[2, 0] = 0 kalman.state_pre[3, 0] = 0 # set kalman transition matrix kalman.transition_matrix[0, 0] = 1 kalman.transition_matrix[1, 1] = 1 kalman.transition_matrix[2, 2] = 1 kalman.transition_matrix[3, 3] = 1 # set Kalman Filter cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1)) cv.SetIdentity(kalman.process_noise_cov,
def __kalman(self): self.kalman = cv.CreateKalman(4, 2, 0) self.kalman_state = cv.CreateMat(4, 1, cv.CV_32FC1) # (phi, delta_phi) self.kalman_process_noise = cv.CreateMat(4, 1, cv.CV_32FC1) self.kalman_measurement = cv.CreateMat(2, 1, cv.CV_32FC1)
def __init__(self, n_lanes, proc_noise_scale, meas_noise_scale, process_cov_parallel=0, proc_noise_type='white'): self.n_lanes = n_lanes self.meas_size = 4 * self.n_lanes self.state_size = self.meas_size * 2 self.contr_size = 0 self.kalman = cv2.CreateKalman(self.state_size, self.meas_size, self.contr_size) #print self.kalman.transition_matrix.cols #print self.kalman.transition_matrix.rows #print self.kalman.transition_matrix.step t2 = np.eye(self.state_size, dtype=np.float32) for t in range(self.kalman.transition_matrix.rows): for tt in range(self.kalman.transition_matrix.cols): self.kalman.transition_matrix[t, tt] = t2[t, tt] #print self.kalman.measurement_matrix for t in range(self.kalman.measurement_matrix.rows): for tt in range(self.kalman.measurement_matrix.cols): self.kalman.measurement_matrix[t, tt] = 0 for i in range(self.meas_size): self.kalman.measurement_matrix[i, i * 2] = 1 if proc_noise_type == 'white': block = np.matrix([[0.25, 0.5], [0.5, 1.]], dtype=np.float32) t2 = (block_diag(*([block] * self.meas_size)) * proc_noise_scale) for t in range(self.kalman.process_noise_cov.rows): for tt in range(self.kalman.process_noise_cov.cols): self.kalman.process_noise_cov[t, tt] = t2[t, tt] #print self.kalman.process_noise_cov[t,tt] ''' if proc_noise_type == 'identity': self.kalman.processNoiseCov = np.eye(self.state_size, dtype=np.float32) * proc_noise_scale ''' for i in range(0, self.meas_size, 2): for j in range(1, self.n_lanes): self.kalman.process_noise_cov[i, i + (j * 8)] = process_cov_parallel self.kalman.process_noise_cov[i + (j * 8), i] = process_cov_parallel #print self.kalman.measurement_noise_cov t2 = np.eye(self.meas_size, dtype=np.float32) * meas_noise_scale for t in range(self.kalman.measurement_noise_cov.rows): for tt in range(self.kalman.measurement_noise_cov.cols): self.kalman.measurement_noise_cov[t, tt] = t2[t, tt] #print self.kalman.error_cov_pre t2 = np.eye(self.state_size, dtype=np.float32) * meas_noise_scale for t in range(self.kalman.error_cov_pre.rows): for tt in range(self.kalman.error_cov_pre.cols): self.kalman.error_cov_pre[t, tt] = t2[t, tt] self.state = cv2.CreateMat(self.state_size, 1, cv2.CV_32FC1) self.meas = cv2.CreateMat(self.meas_size, 1, cv2.CV_32FC1) self.first_detected = False
import math import cv2.cv as cv import sys RADIUS = 0.02 MASS = 0.00275 GRAVITY = 9.82 RHO_A = 1.29 C_D = 0.405 C_M = 0.62 PI = 3.141592653 dT = 0.005 kD = 0.5 * MASS * C_D * RHO_A * PI * RADIUS * RADIUS * dT kM = 0.5 * MASS * C_M * RHO_A * RADIUS * PI * RADIUS * RADIUS * dT kalman = cv.CreateKalman(6, 3, 1) process_noise = cv.CreateMat(6, 1, cv.CV_32FC1) measurement = cv.CreateMat(3, 1, cv.CV_32FC1) control = cv.CreateMat(1, 1, cv.CV_32FC1) rng = cv.RNG(-1) cv.Zero(measurement) 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
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