def __init__(self, mean, covars, fs, hs, symmetries, Q, R, new):
self.new = new
self.version = 2
#Unpack mean vector from objects
self.mean = matrix([[]]).reshape((0,1))
self.dims = []
self.origins = []
self.types = []
self.mean = mean
self.state_dim = 0
for o in mean:
self.dims.append(o.dim)
self.state_dim += o.dim
self.types.append(o.__class__)
#Dimension of each portion of the augmentation
self.process_noise_dim = Q.shape[0]
self.measurement_noise_dim = R.shape[0]
self.dim = self.state_dim + self.process_noise_dim + self.measurement_noise_dim
#Unpack covariances for diagonal from objects
self.covar = append_matrices(covars)
#Augmented covariance matrix
self.covar = append_matrices([self.covar,Q,R])
self.update_functions = fs
self.measurement_predict_functions = hs
self.syms = symmetries
def addObservation(self, h, symmetry, of_obj_indices, noise):
self.syms.append(symmetry)
self.covar = append_matrices((self.covar, noise))
low_noise = self.measurement_noise_dim
high_noise = self.measurement_noise_dim+noise.shape[0]
self.measurement_predict_functions.append(lambda state, noise: h(state, noise, of_obj_indices, range(low_noise, high_noise)))
self.measurement_noise_dim += noise.shape[0]
self.dim += noise.shape[0]
def addObject(self, mean, covar, f, of_obj_indices, noise):
self.mean.append(mean)
self.covar = append_matrices([self.covar[0:self.state_dim,0:self.state_dim], covar, self.covar[self.state_dim:self.state_dim+self.process_noise_dim,self.state_dim:self.state_dim+self.process_noise_dim], noise, self.covar[self.state_dim+self.process_noise_dim:,self.state_dim+self.process_noise_dim:]])
self.state_dim += mean.dim
self.dims.append(mean.dim)
self.types.append(mean.__class__)
low_noise = self.process_noise_dim
high_noise = self.process_noise_dim+noise.shape[0]
new_index = len(self.mean)-1
self.update_functions.append(lambda command, objs, noise: f(command, objs, noise, [new_index]+of_obj_indices, range(low_noise, high_noise)))
self.process_noise_dim += noise.shape[0]
self.dim += mean.dim + noise.shape[0]
import pdb
objects = [Vector(matrix([[1],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[-1],[0]])),Angle.fromRadians(-pi/2)]
covars = [.1*matrix(identity(2)),matrix([[(pi/6)**2]]),.1*matrix(identity(2)),matrix([[(pi/6)**2]])]
def update_function(command, objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+matrix([[(command[0]+noise[0,0])*cos(objects[1].toRadians())],[(command[0]+noise[0,0])*sin(objects[1].toRadians())]])),Angle.fromRadians(pi/12+objects[1].toRadians()+noise[1,0]),Vector(objects[2].v+matrix([[(command[1]+noise[2,0])*cos(objects[3].toRadians())],[(command[1]+noise[2,0])*sin(objects[3].toRadians())]])),Angle.fromRadians(pi/12+objects[3].toRadians()+noise[3,0])]
def measurement_predict_function(objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+noise[0:2]),Angle.fromRadians(objects[1].toRadians()+noise[2,0]),Vector(objects[2].v+noise[3:5]),Angle.fromRadians(objects[3].toRadians()+noise[5,0])]
covar_process_noise = append_matrices((matrix([[.005**2]]),matrix([[(pi/36)**2]]),matrix([[.005**2]]),matrix([[(pi/36)**2]])))
covar_measurement_noise = append_matrices(((.005**2)*matrix(identity(2)),matrix([[(pi/36)**2]]),(.005**2)*matrix(identity(2)),matrix([[(pi/36)**2]])))
k = UnscentedKalmanFilter(objects, covars, update_function, measurement_predict_function, [None,None,None,None], covar_process_noise, covar_measurement_noise,True)
realPos = [Vector(matrix([[1],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[-1],[0]])),Angle.fromRadians(-pi/2)]
for i in xrange(50):
realPos = update_function([pi/12,pi/12], realPos, matrix(random.multivariate_normal([0,0,0,0],covar_process_noise)).getT())
measurement = measurement_predict_function(realPos, matrix(random.multivariate_normal([0,0,0,0,0,0],covar_measurement_noise)).getT())
#k.step([[measurement[0],measurement[2]],[measurement[1],measurement[3]]], [[0,2],[1,3]], [pi/12,pi/12])
k.stateUpdate([pi/12,pi/12])
k.measurementUpdate([[measurement[0],measurement[2]],[measurement[1],measurement[3]]], [[0,2],[1,3]])
print "-------------"
print "real position:"
for i in realPos:
