def update(self, data):
dt = time_difference(self.timestamp, data.get_timestamp())
self.timestamp = data.get_timestamp()
self.kalmanFilter.updateF(dt)
self.updateQ(dt)
self.kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = self.kalmanFilter.getx()
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = self.radar_R
elif data.get_name() == 'lidar':
H = self.lidar_H
Hx = self.lidar_H * x
R = self.lidar_R
self.kalmanFilter.update(z, H, Hx, R)
def kalman(data, kalmanFilter, timestamp):
dt = time_difference(timestamp, data.get_timestamp())
timestamp = data.get_timestamp()
kalmanFilter.F[0, 2], kalmanFilter.F[1, 3] = dt, dt
kalmanFilter.Q = updateQ(dt)
kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = kalmanFilter.x
if data.get_name() == 'radar':
px, py, vx, vy = x[0, 0], x[1, 0], x[2, 0], x[3, 0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy)
H = calculate_jacobian(px, py, vx, vy)
Hx = (np.matrix([[rho, phi, drho]])).T
R = radar_R
elif data.get_name() == 'lidar':
H = lidar_H
Hx = lidar_H * x
R = lidar_R
kalmanFilter.update(z, H, Hx, R)
# if data.get_name() == 'lidar':
# print x, kalmanFilter.x, "lidar"
# elif data.get_name() == 'radar':
# print x, kalmanFilter.x, "radar"
return timestamp
def update(self, data):
dt = time_difference(self.timestamp, data.get_timestamp())
self.timestamp = data.get_timestamp()
self.kalmanFilter.updateF(dt)
self.updateQ(dt)
self.kalmanFilter.predict()
z = np.matrix(data.get_raw()).T
x = self.kalmanFilter.getx()
# print(x)
'''
if data.get_name() == 'radar':
px, py, vx, vy, yaw, dyaw = x[0, 0], x[1, 0], x[2, 0], x[3, 0], x[4,0], x[5,0]
rho, phi, drho = cartesian_to_polar(px, py, vx, vy, yaw, dyaw)
H = calculate_jacobian(px, py, vx, vy, yaw, dyaw)
Hx = (np.matrix([[rho, phi, drho]])).T
R = self.radar_R
'''
if data.get_name() == 'imu':
H = self.imu_H
Hx = self.imu_H * x
R = self.imu_R
elif data.get_name() == 'lidar':
H = self.lidar_H
Hx = self.lidar_H * x
R = self.lidar_R
# print('lidar: ', x[0], x[1])
elif data.get_name() == 'gps':
H = self.gps_H
Hx = self.gps_H * x
R = self.gps_R
# print('gps: ', x[0], x[1])
elif data.get_name() == 'rtk':
H = self.rtk_H
Hx = self.rtk_H * x
R = self.rtk_R
# print('rtk: ', x[0], x[1])
self.kalmanFilter.update(z, H, Hx, R)