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])]
from math import sin, cos, pi
from numpy import matrix, random, identity
from kalman_util import append_matrices, add_noise
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
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)]
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])]
from numpy import matrix, random, identity, linalg
from math import pi, tan, sin, cos, sqrt, atan
from time import clock
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
from kalman_util import add_noise
from filter_system import *
from testPr2 import *
import pdb
objects = [Vector(matrix([[1.1],[-.4],[tZ]])),Angle.fromRadians(0),Vector(matrix([[1.1],[0],[tZ]])),Angle.fromRadians(0),Vector(matrix([[1.3],[0],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[0.],[0.],[0.]])),Angle.fromRadians(0)]
covars = [matrix([[.1**2,0,0],[0,.1**2,0],[0,0,1e-10]]),(.3**2)*matrix(identity(1)),matrix([[.1**2,0,0],[0,.1**2,0],[0,0,1e-10]]),(.3**2)*matrix(identity(1)),matrix([[.07**2,0,0],[0,.03**2,0],[0,0,1e-10]]),(.2**2)*matrix(identity(1)),(.00001**2)*matrix(identity(3)),(.00001**2)*matrix(identity(1))]
covar_process_noise = append_matrices([(.00001**2)*matrix(identity(12)),(.005**2)*matrix(identity(2)),.000000001*matrix(identity(1)),((pi/36)**2)*matrix(identity(1))])
## Mess with this to increase observation variance
#typicalErrProbs.obsVar = 10*typicalErrProbs.obsVar
covar_measurement_noise = append_matrices([typicalErrProbs.obsVar[0:4,0:4],typicalErrProbs.obsVar[0:4,0:4],typicalErrProbs.obsVar[0:4,0:4]])
def state_update_none_vec(command, objs, noise, obj_indices, noise_indices):
return add_noise([objs[obj_indices[0]]],noise[noise_indices[0]:noise_indices[0]+3])[0]
def state_update_none_ang(command, objs, noise, obj_indices, noise_indices):
return add_noise([objs[obj_indices[0]]],noise[noise_indices[0]:noise_indices[0]+1])[0]
def state_update_move_ang(command, objs, noise, obj_indices, noise_indices):
return add_noise([Angle.fromRadians(objs[obj_indices[0]].toRadians()-pi/6*command)],noise[noise_indices[0]])[0]
def measurement_predict_function_angle(objs, noise, obj_indices, noise_indices):
return Angle.fromRadians(objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def update_function_angle(command, objs, noise, obj_indices, noise_indices):
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def measurement_predict_function(objects, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(objects[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def update_function(command, objs, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
from math import sin, cos, pi
from numpy import matrix, random, identity
from kalman_util import append_matrices, add_noise
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
from filter_system import *
import pdb
objects = [Angle.fromRadians(pi/2)]
covars = [matrix([[(pi/1006)**2]])]
def update_function(command, objs, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def measurement_predict_function(objects, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(objects[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
covar_process_noise = matrix([[(pi/36)**2]])
covar_measurement_noise = matrix([[(pi/36)**2]])
fs = FilterSystem()
fs.addObject('ObjA')
