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')