def update(self, z, t):
self.t_current = t
x, vx = None, None
if self.update_dt():
cv.KalmanPredict(self.kal)
self.measurement[0, 0] = z
state_post = cv.KalmanCorrect(self.kal, self.measurement)
x = state_post[0, 0]
vx = state_post[1, 0]
return (x, vx)
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 observation(self, meas):
meas = np.float32([meas])
if not self.initialized:
cv.Copy(cv.fromarray(meas.T.copy()), self.kf.state_post)
cv.Copy(cv.fromarray(meas.T.copy()), self.kf.state_pre)
self.initialized = True
if self.kf.CP == 0:
cv.KalmanPredict(self.kf)
corrected = np.asarray(
cv.KalmanCorrect(self.kf, cv.fromarray(meas.T.copy())))
return corrected.T[0].copy()
def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
kalman = cv.CreateKalman(6, 4)
kalman.transition_matrix[0, 2] = 1
kalman.transition_matrix[0, 4] = 1. / 2
kalman.transition_matrix[1, 3] = 1
kalman.transition_matrix[1, 5] = 1. / 2
kalman.transition_matrix[2, 4] = 1
kalman.transition_matrix[3, 5] = 1
cv.SetIdentity(kalman.measurement_matrix, 1.)
cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
cv.SetIdentity(kalman.error_cov_post, 1.)
p = positions[0]
v = velocities[0]
v2 = velocities[2]
a = (v2 - v).multiply(0.5)
kalman.state_post[0, 0] = p.x
kalman.state_post[1, 0] = p.y
kalman.state_post[2, 0] = v.x
kalman.state_post[3, 0] = v.y
kalman.state_post[4, 0] = a.x
kalman.state_post[5, 0] = a.y
filteredPositions = moving.Trajectory()
filteredVelocities = moving.Trajectory()
measurement = cv.CreateMat(4, 1, cv.CV_32FC1)
for i in xrange(positions.length()):
cv.KalmanPredict(kalman) # no control
p = positions[i]
v = velocities[i]
measurement[0, 0] = p.x
measurement[1, 0] = p.y
measurement[2, 0] = v.x
measurement[3, 0] = v.y
cv.KalmanCorrect(kalman, measurement)
filteredPositions.addPositionXY(kalman.state_post[0, 0],
kalman.state_post[1, 0])
filteredVelocities.addPositionXY(kalman.state_post[2, 0],
kalman.state_post[3, 0])
return (filteredPositions, filteredVelocities)
def correct(self, *corrections):
if self.prediction:
self.measurement = self.prediction
for c in corrections:
self.measurement[corrections.index(c), 0] = c
cv.KalmanCorrect(self.kalman, self.measurement)
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")
def Update(self, z, t=None):
# note: z=(x,y)
if t is not None:
tNew = t
else:
tNew = rospy.Time.now()
if self.initialized:
# Update the state transition matrix for dt.
self.dt = tNew - self.t
self.kal.transition_matrix[0, 2] = self.dt
self.kal.transition_matrix[1, 3] = self.dt
#rospy.logwarn ('KF dt=' + '*' * int(1/(20*self.dt)))
# Kalman Filtering
state_pre = cv.KalmanPredict(self.kal)
if (z is not None) and (self.zf is not None) and (self.dt != 0):
self.measurement[0, 0] = z[0]
self.measurement[1, 0] = z[1]
self.measurement[2, 0] = (z[0] - self.zf[0]) / self.dt
self.measurement[3, 0] = (z[1] - self.zf[1]) / self.dt
state_post = cv.KalmanCorrect(self.kal, self.measurement)
x = state_post[0, 0]
y = state_post[1, 0]
vx = state_post[2, 0]
vy = state_post[3, 0]
else:
x = state_pre[0, 0]
y = state_pre[1, 0]
vx = state_pre[2, 0]
vy = state_pre[3, 0]
#rospy.loginfo('KF z==None -> x,y=%s' % [x,y])
self.zf = z
else: # not initialized.
if (z is not None):
# Set initial conditions
x = z[0]
y = z[1]
vx = 0.0
vy = 0.0
cv.Set2D(self.kal.state_post, 0, 0, z[0])
cv.Set2D(self.kal.state_post, 1, 0, z[1])
cv.Set2D(self.kal.state_post, 2, 0, vx)
cv.Set2D(self.kal.state_post, 3, 0, vy)
rospy.loginfo('KF initialized kalman filter to %s' %
[z[0], z[1], vx, vy])
self.zf = z
self.initialized = True
else:
x = None
y = None
vx = None
vy = None
self.t = tNew
return (x, y, vx, vy)
