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