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