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)
