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 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 SensorFusion: X_INDEX = 0 Y_INDEX = 1 VX_INDEX = 2 VY_INDEX = 3 AX_INDEX = 4 AY_INDEX = 5 def __init__(self): self.resetup([True] * 6) self._kf = KalmanFilter() def resetup(self, maintain_state_flag): self._maintain_state_flag = copy.copy(maintain_state_flag) self._maintain_state_size = self._maintain_state_flag.count(True) print(self._maintain_state_size) print(self._maintain_state_flag) # lidar H will not change even if we change the state we maintain self._lidar_H = np.zeros(self._maintain_state_size * 2).reshape(2, -1) self._lidar_H[0, 0], self._lidar_H[1, 1] = 1.0, 1.0 '''setup F matrix, if we maintain acceleration, it should be reflected in F matrix''' def setup_F(self, dt): self._F = np.identity(self._maintain_state_size) self._F[SensorFusion.X_INDEX, SensorFusion.VX_INDEX] = dt self._F[SensorFusion.Y_INDEX, SensorFusion.VY_INDEX] = dt if self._maintain_state_flag[SensorFusion.AX_INDEX]: self._F[SensorFusion.VX_INDEX, SensorFusion.AX_INDEX] = dt if self._maintain_state_flag[SensorFusion.AY_INDEX]: self._F[SensorFusion.VY_INDEX, SensorFusion.AY_INDEX] = dt def setup_lidar_H(self): self._kf.setH(self._lidar_H) def update_with_lidar_obs(self, lidar_obs): z = np.array([lidar_obs.get_local_px(), lidar_obs.get_local_py()]).reshape(-1, 1) self._kf.setMeasurement(z) self._kf.setH(self._lidar_H) print("lidar_H ", self._lidar_H) self._kf.update() return self._kf.getState(), self._kf.getP() def predict(self, dt, warp): rotation = warp[:2, :2] translation = warp[:2, 2] self._kf.warp(rotation, translation) self.setup_F(dt) self._kf.setF(self._F) self._kf.predict() def update_with_radar_obs(self, radar_obs, use_lat_v=False): z = radar_obs.get_radar_measurement().reshape(-1, 1) if not use_lat_v: z = z[:3, :].reshape(-1, 1) print("radar z", z) self._kf.setMeasurement(z) self.setup_radar_H(use_lat_v) print("radar H", self._kf.getH()) self._kf.update() return self._kf.getState(), self._kf.getP() def setup_radar_H(self, use_lat_v=False): if use_lat_v: self.setup_radar_H_w_lat(self.get_state()) else: self.setup_radar_H_wo_lat(self.get_state()) def cal_radar_observe_wo_lat(self, state): state = copy.copy(state) x, y, vx, vy = state[0], state[1], state[2], state[3] R = np.sqrt(x**2 + y**2) theta = np.arctan2(y, x) dRdt = vx * np.cos(theta) + vy * np.sin(theta) return np.array([R, theta, dRdt]).reshape(-1, 1) def setup_radar_H_wo_lat(self, state): state = copy.copy(state) state.reshape(-1, 1) x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] obs = self.cal_radar_observe_wo_lat(state) R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0] dR_dx = x / R dR_dy = y / R dR_dvx = 0.0 dR_dvy = 0.0 dtheta_dx = -y / (R**2) dtheta_dy = x / (R**2) dtheta_dvx = 0.0 dtheta_dvy = 0.0 dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \ vy * np.cos(theta) * dtheta_dx dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \ vy * np.cos(theta) * dtheta_dy dRdt_dvx = np.sin(theta) dRdt_dvy = np.cos(theta) H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy], [dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy], [dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy]]) self._kf.setH(H) def cal_radar_observe_w_lat(self, state_): state = copy.copy(state_) x, y, vx, vy = state[0], state[1], state[2], state[3] R = np.sqrt(x**2 + y**2) theta = np.arctan2(y, x) dRdt = vx * np.cos(theta) + vy * np.sin(theta) dLdt = -vx * np.sin(theta) + vy * np.cos(theta) return np.array([R, theta, dRdt, dLdt], dtype=float).reshape(-1, 1) def setup_radar_H_w_lat(self, state): state = copy.copy(state) state.reshape(-1, 1) x, y, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] obs = self.cal_radar_observe_w_lat(state) R, theta, dRdt = obs[0, 0], obs[1, 0], obs[2, 0] dR_dx = x / R dR_dy = y / R dR_dvx = 0.0 dR_dvy = 0.0 dtheta_dx = -y / (R**2) dtheta_dy = x / (R**2) dtheta_dvx = 0.0 dtheta_dvy = 0.0 dRdt_dx = -vx * np.sin(theta) * dtheta_dx + \ vy * np.cos(theta) * dtheta_dx dRdt_dy = -vx * np.sin(theta) * dtheta_dy + \ vy * np.cos(theta) * dtheta_dy dRdt_dvx = np.sin(theta) dRdt_dvy = np.cos(theta) dLdt_dx = -vx * np.cos(theta) * dtheta_dx - \ vy * np.sin(theta) * dtheta_dx dLdt_dy = -vx * np.cos(theta) * dtheta_dy - \ vy * np.sin(theta) * dtheta_dy dLdt_dvx = -np.sin(theta) dLdt_dvy = np.cos(theta) H = np.array([[dR_dx, dR_dy, dR_dvx, dR_dvy], [dtheta_dx, dtheta_dy, dtheta_dvx, dtheta_dvy], [dRdt_dx, dRdt_dy, dRdt_dvx, dRdt_dvy], [dLdt_dx, dLdt_dy, dLdt_dvx, dLdt_dvy]]) self._kf.setH(H) def set_maintain_state(self, state_flags): self.resetup(state_flags) def set_P(self, P): self._kf.setP( P[:self._maintain_state_size, :self._maintain_state_size]) def get_P(self): return self._kf.getP() def set_Q(self, Q): self._kf.setQ( Q[:self._maintain_state_size, :self._maintain_state_size]) def get_Q(self): return self._kf.getQ() def set_R(self, R): self._kf.setR(R) def get_R(self): return self._kf.getR() def set_state(self, state): print("state:", state) if len(state) != self._maintain_state_size: print(" STATE dimension not equals to maintain state size") s = [] for i in range(len(self._maintain_state_flag)): if self._maintain_state_flag[i]: s.append(state[i]) s = np.array(s).reshape(-1, 1) self._kf.setState(s) def get_state(self): return self._kf.getState() def get_state_in_lidar_format(self, Twl): state = self.get_state() px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] if len(state) == 6: ax, ay = state[4, 0], state[5, 0] else: ax, ay = 0, 0 return LidarObservation(px, py, vx, vy, Twl=Twl, local_ax=ax, local_ay=ay) def get_state_in_radar_format(self, Twr): state = self.get_state() px, py, vx, vy = state[0, 0], state[1, 0], state[2, 0], state[3, 0] R = np.sqrt(px**2 + py**2) theta = np.arctan2(py, px) range_v = vx * np.cos(theta) + vy * np.sin(theta) lat_v = -vx * np.sin(theta) + vy * np.cos(theta) return RadarObservation(R, theta, range_v, lat_v, Twr)
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()