def control(self, signal):
signal = np.float32([signal])
if not self.initialized:
return np.zeros(1, self.kf.DP)
predicted = np.asarray(
cv.KalmanPredict(self.kf, cv.fromarray(signal.T.copy())))
return predicted.T[0].copy()
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, 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 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 get_prediction(self):
self.prediction = cv.KalmanPredict(self.kalman)
return self.prediction
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)
prediction = cv.KalmanPredict(kalman)
predict_angle = prediction[0, 0]
predict_pt = calc_point(predict_angle)
cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0),
cv.RealScalar(sqrt(kalman.measurement_noise_cov[0, 0])))
# generate measurement
cv.MatMulAdd(kalman.measurement_matrix, state, measurement,
measurement)
measurement_angle = measurement[0, 0]
measurement_pt = calc_point(measurement_angle)
# plot points
def draw_cross(center, color, d):
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)