class FusionEKF:
"""
Gets inputs from multiple sources and uses a Kalman Filter to estimate the state of the system
The state of the system is:
X = {pD, dotpD, attD, dotattD, pJ, dotpJ, headJ, GSPoffsetJ, compassOffsetJ}
The inputs are:
- Drone's position in local ENU
- Drone's velocity in local ENU
- Drone's attitude
- Drone's angular velocities
- Jetyak's position in local ENU
- Jetyak's heading
- Jetyak's position in the Drone's body frame
"""
def __init__(self, F, P, N, rate):
self.kalmanF = KalmanFilter()
self.F = F
self.P = P
self.N = N
self.n = np.shape(F)[0]
self.X = np.matrix(np.zeros((self.n, 1)))
self.dt = 1 / rate
self.isInit = False
self.X[0:3] = np.nan # Drone's position
self.X[14:17] = np.nan # Jetyak's position
self.X[19:24] = np.nan # Jetyak's GPS and heading offset
def initialize(self, dataPoint):
if dataPoint.getID() == 'dgps':
self.X[0] = dataPoint.getZ().item(0)
self.X[1] = dataPoint.getZ().item(1)
self.X[2] = dataPoint.getZ().item(2)
elif dataPoint.getID() == 'jgps':
self.X[6] = dataPoint.getZ().item(0)
self.X[7] = dataPoint.getZ().item(1)
self.X[8] = dataPoint.getZ().item(2)
elif dataPoint.getID() == 'tag':
if (not (np.isnan(self.X[0]) or
np.isnan(self.X[6]))):
self.X[11] = self.X[6] - self.X[0] - dataPoint.getZ().item(0)
self.X[12] = self.X[7] - self.X[1] - dataPoint.getZ().item(1)
self.X[13] = self.X[8] - self.X[2] - dataPoint.getZ().item(2)
self.X[14] = dataPoint.getZ().item(3)
self.kalmanF.initialize(self.X, self.F, self.P, self.N)
self.kalmanF.updateF(self.dt)
self.kalmanF.updateQ()
self.kalmanF.predict()
self.isInit = True
print 'Colocalization filter initialized'
def process(self, dataPoint):
if not self.isInit:
self.initialize(dataPoint)
return None, None
else:
r = None
P = None
# KF Correction Step
r, P = self.kalmanF.correct(dataPoint.getZ(), dataPoint.getH(), dataPoint.getR(), dataPoint.getChi())
if r is None:
print "A ", dataPoint.getID(), " measurement was rejected"
self.X = self.kalmanF.getState()
return r, P
def getState(self):
if self.isInit:
self.kalmanF.predict()
return self.X
else:
return None
def resetFilter(self):
self.isInit = False
self.X = np.matrix(np.zeros((self.n, 1)))
self.X[0:3] = np.nan # Drone's position
self.X[14:17] = np.nan # Jetyak's position
self.X[19:24] = np.nan # Jetyak's GPS and heading offset
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))
self.F[3:6, 3:6] = 0.975 * np.matrix(np.eye(3))
self.F[9:12, 9:12] = 0.975 * np.matrix(np.eye(3))
# Initial Process Matrix
self.P = np.asmatrix(1.0e3 * np.eye(self.n))
# Process Noise Level
self.N = 1.0e-3
# 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.0 * 1.0e-3 * np.matrix(np.eye(3))
self.Ropti[3:6, 3:6] = 1.0 * 1.0e-2 * 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
self.lastTime = timeNow
# Prediction
self.kalmanF.updateF(dt)
self.kalmanF.updateQ()
self.kalmanF.predict()
# Update state
self.X = self.kalmanF.getState()
return self.X
else:
return None
class FilterNode():
def __init__(self, ):
rp.init_node("filter")
# Get rate of state publisher
self.rate = rp.get_param("rate", 100)
# Get VRPN client topic
self.vrpn_topic = rp.get_param("vrpn_topic")
# 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-3
# 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-5 * np.matrix(np.eye(3))
# Initialize Kalman Filter
self.kalmanF = KalmanFilter()
self.isInit = False
self.lastTime = None
# Set up Subscriber
self.vrpn_sub = rp.Subscriber(self.vrpn_topic,
PoseStamped,
self.state_callback,
queue_size=1)
# Set up Publisher
self.state_pub = rp.Publisher("opti_state/pose",
PoseStamped,
queue_size=1)
self.state_rate_pub = rp.Publisher("opti_state/rates",
TwistStamped,
queue_size=1)
# Create thread for publisher
t = threading.Thread(target=self.statePublisher)
t.start()
rp.spin()
def state_callback(self, msg):
attiQ = Quaternion(msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
rpy = quat2rpy(attiQ)
pose = np.matrix([[msg.pose.position.x], [msg.pose.position.y],
[msg.pose.position.z], [rpy[0]], [rpy[1]], [rpy[2]]])
if self.isInit:
timeNow = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs
dt = timeNow - 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 = timeNow
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 = msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs
self.isInit = True
def statePublisher(self):
r = rp.Rate(self.rate)
while not rp.is_shutdown():
if self.isInit:
timeNow = rp.Time.now()
stateMsg = PoseStamped()
stateMsg.header.stamp = timeNow
stateMsg.header.frame_id = 'optitrack'
stateMsg.pose.position.x = self.X.item(0)
stateMsg.pose.position.y = self.X.item(1)
stateMsg.pose.position.z = self.X.item(2)
q = rpy2quat(self.X.item(6), self.X.item(7), self.X.item(8))
stateMsg.pose.orientation.x = q.x
stateMsg.pose.orientation.y = q.y
stateMsg.pose.orientation.z = q.z
stateMsg.pose.orientation.w = q.w
twistMsg = TwistStamped()
twistMsg.header.stamp = timeNow
twistMsg.header.frame_id = 'optitrack'
twistMsg.twist.linear.x = self.X.item(3)
twistMsg.twist.linear.y = self.X.item(4)
twistMsg.twist.linear.z = self.X.item(5)
twistMsg.twist.angular.x = self.X.item(9)
twistMsg.twist.angular.y = self.X.item(10)
twistMsg.twist.angular.z = self.X.item(11)
self.state_pub.publish(stateMsg)
self.state_rate_pub.publish(twistMsg)
r.sleep()
