def __init__(self, processNoiseCovariance=1e-2, 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
'''
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, process_noise_cov=(1.0, 800.0)):
self.kal = cv.CreateKalman(2, 1, 0)
cv.SetIdentity(self.kal.transition_matrix, 1.0)
cv.SetIdentity(self.kal.measurement_matrix, 1.0)
cv.SetIdentity(self.kal.process_noise_cov, 1.0)
self.kal.process_noise_cov[0, 0] = process_noise_cov[0]
self.kal.process_noise_cov[1, 1] = process_noise_cov[1]
cv.SetIdentity(self.kal.measurement_noise_cov, 1.0)
cv.SetIdentity(self.kal.error_cov_post, 1.0)
self.measurement = cv.CreateMat(1, 1,
cv.GetElemType(self.kal.state_pre))
self.t_previous = None
def __init__(self,
initial_val=[0],
p_noise=[1e-2],
m_noise=[1e-3],
m_mat=[1],
ecv=[1]):
self.kalman = cv.CreateKalman(len(initial_val), len(initial_val), 0)
self.measurement = cv.CreateMat(len(initial_val), 1, cv.CV_32FC1)
self.prediction = None
cv.Zero(self.measurement)
cv.SetIdentity(self.kalman.measurement_matrix, cv.RealScalar(*m_mat))
cv.SetIdentity(self.kalman.process_noise_cov, cv.RealScalar(*p_noise))
cv.SetIdentity(self.kalman.measurement_noise_cov,
cv.RealScalar(*m_noise))
cv.SetIdentity(self.kalman.error_cov_post, cv.RealScalar(*ecv))
for v in initial_val:
self.kalman.state_post[initial_val.index(v), 0] = v
self.kalman.state_pre[initial_val.index(v), 0] = v
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 __init__(self):
self.initialized = False
self.kal = cv.CreateKalman(4, 4, 0)
cv.SetIdentity(self.kal.transition_matrix)
cv.SetIdentity(self.kal.measurement_matrix)
cv.SetIdentity(self.kal.process_noise_cov, 1.0)
self.kal.process_noise_cov[2, 2] = 0.5
self.kal.process_noise_cov[3, 3] = 0.5
cv.SetIdentity(self.kal.measurement_noise_cov, 80.0)
self.kal.measurement_noise_cov[0, 0] = 5E-5 #15.341
self.kal.measurement_noise_cov[0, 1] = 8E-6 # 2.354
self.kal.measurement_noise_cov[1, 0] = 8E-6 #1E-2 # 2.354
self.kal.measurement_noise_cov[
1, 1] = 5E-5 # Likely to be similar to the x value.
self.kal.measurement_noise_cov[2, 2] = 40.0 #0.22291
self.kal.measurement_noise_cov[3, 3] = 40.0 #0.21742
self.measurement = cv.CreateMat(4, 1,
cv.GetElemType(self.kal.state_pre))
self.t = None
self.zf = None
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 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
def __init__(self, cov, dynam_params, measure_params, control_params=0):
self.kf = cv.CreateKalman(dynam_params, measure_params, control_params)
cv.SetIdentity(self.kf.measurement_matrix, cv.RealScalar(1))
self.set_cov(cv.fromarray(cov))
