class FilterNode():
def __init__(self):
# Number of states
self.n = 12
# System state
self.X = np.matrix(np.zeros((self.n, 1)))
# Initial State Transition Matrix
self.F = np.asmatrix(np.eye(self.n))
# Initial Process Matrix
self.P = np.asmatrix(1.0e3 * np.eye(self.n))
# Process Noise Level
self.N = 1.0e-2
# Initialize H and R matrices for optitrack pose
self.Hopti = np.matrix(np.zeros((6, self.n)))
self.Hopti[0:3, 0:3] = np.matrix(np.eye(3))
self.Hopti[3:6, 6:9] = np.matrix(np.eye(3))
self.Ropti = np.matrix(np.zeros((6, 6)))
self.Ropti[0:3, 0:3] = 1.0e-3 * np.matrix(np.eye(3))
self.Ropti[3:6, 3:6] = 1.0e-3 * np.matrix(np.eye(3))
# Initialize Kalman Filter
self.kalmanF = KalmanFilter()
self.isInit = False
self.lastTime = None
def state_update(self, T, R, mTime):
attiQ = Quaternion(R[0], R[1], R[2], R[3])
rpy = quat2rpy(attiQ)
pose = np.matrix([[T[0]], [T[1]], [T[2]], [rpy[0]], [rpy[1]],
[rpy[2]]])
if self.isInit:
dt = mTime - self.lastTime
# Prediction
self.kalmanF.updateF(dt)
self.kalmanF.updateQ()
self.kalmanF.predict()
# Correction
self.kalmanF.correct(pose, self.Hopti, self.Ropti)
# Update state
self.X = self.kalmanF.getState()
self.lastTime = mTime
else:
# Initialize state
self.X[0] = pose[0]
self.X[1] = pose[1]
self.X[2] = pose[2]
self.X[6] = pose[3]
self.X[7] = pose[4]
self.X[8] = pose[5]
# Initialize filter
self.kalmanF.initialize(self.X, self.F, self.P, self.N)
self.lastTime = mTime
self.isInit = True
def get_state(self):
if self.isInit:
timeNow = time.time()
dt = timeNow - self.lastTime
# Get a prediction
self.kalmanF.updateF(dt)
return self.kalmanF.get_prediction()
else:
return None
