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
