class ApriltagsKalman(Apriltag, object):
def __init__(self):
super(ApriltagsKalman, self).__init__()
self.kf_x = KalmanFilter()
self.kf_y = KalmanFilter()
self.kf_z = KalmanFilter()
#def set(self, iid, ppos, qq, pre_pos, pre_q, size):
def update(self, iid, ppos, qq, pre_pos, pre_q):
if (self.id == iid):
v = self.update_pose(ppos, qq)
self.update_velocity(v, qq, pre_q)
else:
raise valueerror("id is not not miss-match")
sys.exit()
def reset_velocity(self):
self.speed = np.zeros((3, 1))
self.accel = np.zeros((3, 1))
self.d_quaternion = Quaternion(0., 0., 0., 1.) #(w,x,y,z)
self.kf_x.reset()
self.kf_y.reset()
self.kf_z.reset()
def update_pose(self, ppos, qq):
x = self.kf_x.test(ppos[0])
y = self.kf_y.test(ppos[1])
z = self.kf_z.test(ppos[2])
self.pose = np.array(
[np.asscalar(x[0]),
np.asscalar(y[0]),
np.asscalar(z[0])])
self.quaternion = qq
return np.array([\
np.asscalar(x[1]),\
np.asscalar(y[1]),\
np.asscalar(z[1])\
])
def update_velocity(self, v, qq, pre_q):
self.speed = v
self.d_quaternion = qq * pre_q.inverse
