def test(x, y, z, qcov, rcov):
# Initializations
dt = 1 # Measurement interval (s)
I = lambda x: identity(x)
A = zeros((9, 9)) # Transition matrix
A[0:3, 0:3] = I(3)
A[0:3, 3:6] = I(3) * dt
A[0:3, 6:9] = I(3) * 0.5 * dt * dt
A[3:6, 3:6] = I(3)
A[3:6, 6:9] = I(3) * dt
A[6:9, 6:9] = I(3)
H = zeros((3, 9)) # Measurement matrix
H[0:3, 0:3] = I(3)
Q = identity(9) * qcov # Transition covariance
R = identity(3) * rcov # Noise covariance
B = identity(9) # Control matrix
kf = KalmanFilter(A, H, Q, R, B)
# Run through the dataset
n = len(x)
xkf, ykf, zkf = zeros(n), zeros(n), zeros(n)
for i in xrange(n):
kf.correct(array([x[i], y[i], z[i]]))
kf.predict(array([]))
Skf = kf.getCurrentState()
xkf[i], ykf[i], zkf[i] = Skf[0], Skf[1], Skf[2]
return xkf, ykf, zkf
class KalmanTrack:
"""
This class represents the internal state of individual tracked objects observed as bounding boxes.
"""
def __init__(self, initial_state):
"""
Initialises a tracked object according to initial bounding box.
:param initial_state: (array) single detection in form of bounding box [X_min, Y_min, X_max, Y_max]
"""
self.kf = KalmanFilter(dim_x=7, dim_z=4)
# Transition matrix
self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 0, 1],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]])
# Transformation matrix (Observation to State)
self.kf.H = np.array([[1, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0]])
self.kf.R[2:, 2:] *= 10. # observation error covariance
self.kf.P[4:, 4:] *= 1000. # initial velocity error covariance
self.kf.P *= 10. # initial location error covariance
self.kf.Q[-1, -1] *= 0.01 # process noise
self.kf.Q[4:, 4:] *= 0.01 # process noise
self.kf.x[:4] = xxyy_to_xysr(initial_state) # initialize KalmanFilter state
def project(self):
"""
:return: (ndarray) The KalmanFilter estimated object state in [x1,x2,y1,y2] format
"""
return xysr_to_xxyy(self.kf.x)
def update(self, new_detection):
"""
Updates track with new observation and returns itself after update
:param new_detection: (np.ndarray) new observation in format [x1,x2,y1,y2]
:return: KalmanTrack object class (itself after update)
"""
self.kf.update(xxyy_to_xysr(new_detection))
return self
def predict(self):
"""
:return: ndarray) The KalmanFilter estimated new object state in [x1,x2,y1,y2] format
"""
if self.kf.x[6] + self.kf.x[2] <= 0:
self.kf.x[6] *= 0.0
self.kf.predict()
return self.project()
def kalman_angle():
global kAngle_g
T = 0.01
lamC = 1.0 #variance for compass
lamGPS = 1.0 #variance for gps bearing
lamGyro = 1.0 #variance for gyroscope
sig = 1.0 #variance for model
sig2 = sig**2
lamC2 = lamC**2
lamGPS2 = lamGPS**2
lamGyro2 = lamGyro**2
(A, H, Q, R) = create_model_parameters(T, sig2, lamC2) #initialize evolution, measurement, model error, and measurement error
# initial state
x = np.array([0, 0.1]) #starting velocity and position
P = 0 * np.eye(2) #starting probability 100%
kalman_filter = KalmanFilter(A, H, Q, R, x, P) #create the weights filter
lastTime = time.time()
startTime = lastTime
lastOmega = 0
lastAngle = 0
thetaCoord_l = 0 #local variable to prevent race conditions
lastBearing = 0
gpsTrust = 1.0
while True:
while (omega_g == lastOmega or thetaCoord_g == lastAngle): #check if there were updates to the compass
time.sleep(0.01) #if no updates then wait a bit
thetaCoord_l = thetaCoord_g
lastAngle = thetaCoord_l
if (kalman_filter._x[0] - thetaCoord_l) > 180: #make sure the filter knows that we crossed the pole
kalman_filter._x[0] = kalman_filter._x[0] - 360
elif (thetaCoord_l - kalman_filter._x[0]) > 180:
kalman_filter._x[0] = kalman_filter._x[0] + 360
kalman_filter.predict() #evolve the state and the error
kalman_filter.update((thetaCoord_l,omega_g),(lamC2,lamGyro2)) #load in 2 measurement values
(x, P) = kalman_filter.get_state() #return state
dt = time.time() - lastTime #calculate dt
lastTime = time.time()
kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps
kAngle_g = x[0]%360 #because the model portion is estimating based on that.
# while (omega_g == lastOmega or gpsTheta_g == lastBearing): #check if there were updates to the gps bearing
# time.sleep(0.01) #if no updates then wait a bit
gpsTheta_l = gpsTheta_g
lastBearing = gpsTheta_l
if (kalman_filter._x[0] - gpsTheta_l) > 180: #make sure the filter knows that we crossed the pole
kalman_filter._x[0] = kalman_filter._x[0] - 360
elif (gpsTheta_l - kalman_filter._x[0]) > 180:
kalman_filter._x[0] = kalman_filter._x[0] + 360
lamGPS2 = gpsTrust/gpsDistance_g
kalman_filter.predict() #evolve the state and the error
kalman_filter.update((gpsTheta_l,omega_g),(lamGPS2,lamGyro2)) #load in 2 measurement values
(x, P) = kalman_filter.get_state() #return state
dt = time.time() - lastTime #calculate dt
lastTime = time.time()
kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps
kAngle_g = x[0]%360 #because the model portion is estimating based on that.
class UltraSoundFilter(object):
def __init__(self, us):
self.ultra_sound = us
self.kalman_filter = None
self.last_update_time = None
self.calibration_time = 3.0
self.calibration_range = None
self.calibration_std = None
def calibrate(self):
""" Run initialization procedure on empty scene.
Determine mean and std of dists within calibration_time.
Can block for a while."""
ranges = []
start_time = time.time()
while time.time() - start_time <= self.calibration_time:
dist = self.ultra_sound.get_distance()
time.sleep(0.05)
if dist is not None:
ranges.append(dist)
if len(ranges) <= 1:
raise RuntimeError("No valid ranges during calibration.")
return
self.calibration_range = sum(ranges) / len(ranges)
sqr_error = [(dist - self.calibration_range)**2 for dist in ranges]
self.calibration_std = math.sqrt(sum(sqr_error) / (len(ranges) - 1))
logger.info(
f"Calibrated range as {self.calibration_range:.2f}m +- {self.calibration_std:.3f}m"
)
self.kalman_filter = KalmanFilter(self.calibration_range,
self.calibration_std)
def update(self):
""" Update the internal feature based on sensor data. """
cur_dist = self.ultra_sound.get_distance()
now = time.time()
if cur_dist is None:
return
if self.last_update_time is None:
delta_t = 0
else:
delta_t = now - self.last_update_time
self.kalman_filter.predict(delta_t)
self.kalman_filter.correct(cur_dist)
self.last_update_time = now
def get_distance(self):
if self.kalman_filter is None:
return None
return self.kalman_filter.distance()
class Tracking(object):
def __init__(self):
self.tracking_list = []
self.data_association = DataAssociation()
self.tracking_filter = KalmanFilter()
pass
def update(self,object_list,timeframe):
if not self.tracking_list:
for object in object_list:
self.init_track(object)
else:
self.tracking_filter.predict(self.tracking_list,timeframe)
asso = self.data_association.nearest_neighbor(object_list,self.tracking_list)
self.tracking_filter.update(object_list,self.tracking_list,asso)
def init_track(self,object):
tr = Track(object)
self.tracking_list.append(tr)
def number_of_tracks(self):
return len(self.tracking_list)
class TestKalmanFilter(unittest.TestCase):
def setUp(self):
A = np.eye(2)
H = np.eye(2)
Q = np.eye(2)
R = np.eye(2)
x_0 = np.array([1, 1])
P_0 = np.eye(2)
self.kalman_filter = KalmanFilter(A, H, Q, R, x_0, P_0)
def test_predict(self):
self.kalman_filter.predict()
(x, P) = self.kalman_filter.get_state()
self.assertTrue((x == np.array([1, 1])).all())
self.assertTrue((P == 2 * np.eye(2)).all())
def test_update(self):
self.kalman_filter.update(np.array([0, 0]))
(x, P) = self.kalman_filter.get_state()
self.assertTrue((x == np.array([0.5, 0.5])).all())
self.assertTrue((P == 0.5 * np.eye(2)).all())
class Tracklet:
STATUS_BORN = 0
STATUS_TRACKING = 1
STATUS_LOST = 2
def __init__(self, global_id, x, y, confidence, timestamp):
self.id = global_id
self.last_update = timestamp
self.estimator = KalmanFilter(x, y, 1 - confidence)
self.status = Tracklet.STATUS_BORN
def get_tracklet_info(self):
return {
'id': self.id,
'x': self.estimator.mu[0],
'y': self.estimator.mu[2],
'v': sqrt(self.estimator.mu[1]**2 + self.estimator.mu[3]**2),
'phi': atan2(self.estimator.mu[1], self.estimator.mu[3]),
'sigma_x': self.estimator.sigma[0][0],
'sigma_y': self.estimator.sigma[2][2],
'status': self.status
}
def estimate_position_at(self, timestamp):
estimate = self.estimator.estimate_at(timestamp - self.last_update)[0]
return estimate[0], estimate[2]
def predict(self, timestamp):
self.estimator.predict(math.fabs(self.last_update - timestamp))
self.last_update = timestamp
def update(self, detection, confidence):
self.estimator.correct(detection, 1 - confidence)
def similarity(self, coordinates, timestamp):
return euclidean(self.estimate_position_at(timestamp), coordinates)
def confidence(self):
return self.estimator.sigma[0][0] * self.estimator.sigma[2][2]
class EM:
def __init__(self):
self.kf = KalmanFilter()
def find_Q(self, data=None):
log_likelihoods = []
for i in range(0, 20):
x_posteriors, x_predictions, cov = self.run(data=data)
log_likelihoods.append(self.calculate_log_likelihood(x_posteriors, cov, data))
self.m_step(x_predictions, x_posteriors, data)
return log_likelihoods
def run(self, data=None):
x_posteriors, x_predictions, cov = [], [], []
for z in data:
self.kf.predict()
x_predictions.append(self.kf.x)
self.kf.update(z)
x_posteriors.append(self.kf.x)
cov.append(self.kf.P)
x_posteriors, x_predictions, cov = np.array(x_posteriors), np.array(x_predictions), np.array(cov)
return x_posteriors, x_predictions, cov
def calculate_log_likelihood(self, x_posteriors, cov, measurements):
log_likelihood = 0
for i in range(0, len(cov)):
S = np.dot(np.dot(self.kf.H, cov[i]), self.kf.H.T) + self.kf.R
state_posterior_in_meas_space = np.dot(self.kf.H, x_posteriors[i]).squeeze()
distribution = multivariate_normal(mean=state_posterior_in_meas_space, cov=S)
log_likelihood += np.log(distribution.pdf(measurements[i]))
return log_likelihood
def m_step(self, x_predictions, x_posteriors, measurements):
self.kf = KalmanFilter()
self.kf.Q = np.cov((x_posteriors - x_predictions).squeeze().T, bias=True)
self.kf.R = np.cov((measurements.T - np.dot(self.kf.H, x_posteriors.squeeze().T)), bias=True)
class fans_kalman(object):
def __init__(self, initial_x0, sigma_w, sigma_v):
self.sigma_w = sigma_w
self.sigma_v = sigma_v
self.define_kalman_parameters(initial_x0)
self.define_kalman_filter()
def define_kalman_parameters(self, initial_x0):
'initialize the parameters of kalman filter here'
dt = 1
self.F = np.array([[1, dt], [0, 1]])
self.H = np.array([1, 0]).reshape(1, 2)
self.Q = np.array([[self.sigma_w, 0.0], [0.0, self.sigma_w]])
self.R = np.array([self.sigma_v]).reshape(1, 1)
self.P = np.array([[10., 0], [0, 10.]])
'Notice that, the inital_x0 here actually is a state, which is a 2-d array'
self.initial_x0 = np.array([[initial_x0], [0]])
def define_kalman_filter(self):
self.kf = KalmanFilter(F=self.F,
H=self.H,
Q=self.Q,
R=self.R,
P=self.P,
x0=self.initial_x0)
def predict_interface(self, new_measurement):
'predict the value, and update the kalman filter'
if new_measurement is not None:
self.kf.update(new_measurement)
'Otherwise, skip the measurement-update step...just perform the time-update'
res = np.dot(self.H, self.kf.predict())[0]
'res is a scalar here'
return res
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 KalmanFilterNode(object):
"""
ROS node implementation of Kalman filter.
This node subscribes to a list of all existing Sphero's positions
broadcast from OptiTrack system, associates one of them to the Sphero in
the same namespace and uses Kalman filter to output steady position and
velocity data for other nodes.
"""
def sensor_callback(self, data):
"""Process received positions data and return Kalman estimation.
:type data: Odometry
"""
if self.initial_run:
# Initialize Kalman filter and estimation.
initial_pos = data.pose.pose
self.filter = KalmanFilter(1.0 / self.sub_frequency, initial_pos)
self.X_est = Odometry()
self.X_est.pose.pose = initial_pos
self.initial_run = False
else:
X_measured = data.pose.pose
time = data.header.stamp
# If measurement data is not available, use only prediction step.
# Else, use prediction and update step.
if X_measured is None:
self.X_est = self.filter.predict()
else:
self.X_est = self.filter.predict_update(X_measured)
self.X_est.header.stamp = time
if self.debug_enabled:
self.debug_pub.publish(self.X_est)
def __init__(self):
"""Initialize agent instance, create subscribers and publishers."""
# Initialize class variables.
self.sub_frequency = rospy.get_param('/data_stream_freq')
self.pub_frequency = rospy.get_param('/ctrl_loop_freq')
self.debug_enabled = rospy.get_param('/debug_kalman')
self.X_est = None
# Create publishers.
pub = rospy.Publisher('odom_est',
Odometry,
queue_size=self.pub_frequency)
if self.debug_enabled:
self.debug_pub = rospy.Publisher('debug_est',
Odometry,
queue_size=self.sub_frequency)
# Create subscribers.
self.initial_run = True
rospy.Subscriber('odom',
Odometry,
self.sensor_callback,
queue_size=self.sub_frequency)
# Don't try to publish anything before variable is set.
while self.X_est is None:
rospy.sleep(0.1)
# Main while loop.
rate = rospy.Rate(self.pub_frequency)
while not rospy.is_shutdown():
pub.publish(self.X_est)
rate.sleep()
delta_t = 0.5 # predictions are going to be made for 0.5 seconds in the future
A = np.array([[1, delta_t], [0, 1]])
B = np.zeros((2, 2))
G = np.identity(2)
H = np.array([[1, 0]])
sigma_w = 0.1 # 10cm = standard deviation of position prediction noise for one time step of dt seconds
Q = (sigma_w**2) * np.identity(2)
sigma_n = 0.2 # 20cm = standard deviation of position measurement noise
R = (sigma_n**2) * np.identity(1)
x_init = np.array([[0, 5]]).transpose()
sigma_p = 1 # 10cm = uncertainty about initial position
sigma_v = 20 # 20m/s = uncertainty about initial velocity
Sigma_init = np.array([[sigma_p**2, 0], [0, sigma_v**2]])
kf = KalmanFilter(A, B, G, H, Q, R, x_init, Sigma_init)
plot_mean_and_covariance(kf.x, kf.Sigma)
kf.predict()
plot_mean_and_covariance(kf.x, kf.Sigma)
kf.update(z=5)
plot_mean_and_covariance(kf.x, kf.Sigma)
np.random.seed(21)
(A, H, Q, R) = create_model_parameters()
K = 20
# initial state
x = np.array([0, 0.5, 0, 0.5])
P = 0 * np.eye(4)
(state, meas) = simulate_system(K, x)
kalman_filter = KalmanFilter(A, H, Q, R, x, P)
est_state = np.zeros((K, 4))
est_cov = np.zeros((K, 4, 4))
for k in range(K):
kalman_filter.predict()
kalman_filter.update(meas[k, :])
(x, P) = kalman_filter.get_state()
est_state[k, :] = x
est_cov[k, ...] = P
plt.figure(figsize=(7, 5))
plt.plot(state[:, 0], state[:, 2], '-bo')
plt.plot(est_state[:, 0], est_state[:, 2], '-ko')
plt.plot(meas[:, 0], meas[:, 1], ':rs')
plt.xlabel('x [m]')
plt.ylabel('y [m]')
plt.legend(['true state', 'inferred state', 'observed measurement'])
plt.axis('square')
plt.tight_layout(pad=0)
log.new_log('actual')
log.new_log('time')
log.new_log('covariance')
log.new_log('moving average')
# Moving average for measurements
moving_avg = MovingAverage(15)
# Number of iterations to perform
iterations = 100
for i in range(0, iterations):
# Generate a random acceleration
w = matrix(random.multivariate_normal([0.0, 0.0], Q)).T
# Predict
(X, P) = kf.predict(X, P, w)
# Update
(X, P) = kf.update(X, P, Z)
# Update the actual position
A = F * A + w
# Synthesise a new noisy measurement distributed around the real position
Z = matrix([[random.normal(A[0, 0], sigma_z)], [0.0]])
# Update the moving average with the latest measured position
moving_avg.update(Z[0, 0])
# Update the log for plotting later
log.log('measurement', Z[0, 0])
log.log('estimate', X[0, 0])
log.log('actual', A[0, 0])
log.log('time', i * dt)
log.log('covariance', P[0, 0])
log.log('moving average', moving_avg.getAvg())
[0, 0, 0, sigma_v**2]])
x_init = np.array([[0, 2, 2, 5]]).transpose()
kf = KalmanFilter(A, B, G, H, Q, R, x_init, Sigma_init)
u = np.array([[0, -0.5*g*delta_t**2, 0, -g*delta_t]]).transpose()
x_estimated = [x_init]
cov_estimated = [Sigma_init]
x_real_list = [np.array([[0, 0, 5, 10]]).transpose()]
x_real = x_real_list[0]
z_list = []
for i in xrange(100):
kf.predict(u)
# simulate noisy observation
x_real = A.dot(x_real)+u
z = x_real[0:2, :] + np.random.multivariate_normal(np.zeros((2,)), R).reshape((2,1))
kf.update(z)
x_estimated.append(kf.x)
cov_estimated.append(kf.Sigma)
x_real_list.append(x_real)
z_list.append(z)
plt.figure()
px = np.array([ pv[0, 0] for pv in x_real_list])
py = np.array([ pv[1, 0] for pv in x_real_list])
# init
x = np.array([0, 0])
P = np.array([[0, 0], [0, 0]])
kf = KalmanFilter(F, H, Q, R)
cart = Cart()
vs = [0] # filterd v
ws = [0] # true v
xs = [0] # filterd x
ys = [0] # true x
zs = [0] # observed x
for _ in range(100):
y, z, w = cart()
x, P = kf.predict(x, P)
x, P = kf.update(x, P, z)
vs.append(x[1])
ws.append(w)
xs.append(x[0])
ys.append(y)
zs.append(z)
# plot graph
figs, axes = plt.subplots(1, 2)
axes[1].plot(vs, label='filterd v', marker='.')
axes[1].plot(ws, label='true v', marker='x')
axes[0].plot(xs, label='filterd x', marker='.')
axes[0].plot(ys, label='true x', marker='x')
axes[0].plot(zs, label='observed x', marker='+')
lastYTilt = evDevValue
elif evDevCode == WACOM_EVCODE_DISTANCE:
lastDistance = evDevValue
elif evDevCode == WACOM_EVCODE_PRESSURE:
if not ONLY_DEBUG:
if not mouseButtonPressed and evDevValue > SCREEN_DRAW_BUTTON_PRESSURE:
mouse.press(Button.left)
mouseButtonPressed = True
elif mouseButtonPressed and evDevValue <= SCREEN_DRAW_BUTTON_PRESSURE:
mouse.release(Button.left)
mouseButtonPressed = False
lastPressure = evDevValue
if ONLY_DEBUG:
print('XPos: %5d | YPos: %5d | XTilt: %5d | YTilt: %5d | Distance: %3d | Pressure: %4d' % (lastXPos, lastYPos, lastXTilt, lastYTilt, lastDistance, lastPressure))
else:
screenY = SCREEN_DRAW_AREA_FROM_X + (WACOM_WIDTH - lastXPos) * ratioX # (X doesn't need to invert)
screenX = SCREEN_DRAW_AREA_FROM_Y + (WACOM_HEIGHT - lastYPos) * ratioY # (Y has to be inverted)
currentMeasurement = np.array([[np.float32(screenX)], [np.float32(screenY)]])
currentPrediction = kalman.predict()
cmx, cmy = currentMeasurement[0], currentMeasurement[1]
cpx, cpy = currentPrediction[0], currentPrediction[1]
kalman.correct(currentMeasurement)
mouseMoveAbs(int(np.round(cpx)), int(np.round(cpy)))
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()
def AMM_predict(self,
interpolated_track_dict,
x_in=np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose()):
# ========== parameter setting ========== #
x_in = np.mat([[0.0, 0.0, 0.0, 0.0]]).transpose()
# print('x_in',x_in)
P_in = np.mat([[100.0, 0, 0, 0], [0, 100.0, 0, 0], [0, 0, 100.0, 0],
[0, 0, 0, 100.0]])
H_in = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]])
R_in = np.mat([[0.1, 0], [0, 0.1]])
## tuning ?
t = 0.5
t_2 = t * t
t_3 = t_2 * t
t_4 = t_3 * t
alpha_ax_2 = 2.25 * 2.25
alpha_ay_2 = 2.25 * 2.25
Q_in = np.mat([[t_4 / 4 * alpha_ax_2, t_3 / 2 * alpha_ax_2, 0, 0],
[t_3 / 2 * alpha_ax_2, t_2 * alpha_ax_2, 0, 0],
[0, 0, t_4 / 4 * alpha_ay_2, t_3 / 2 * alpha_ay_2],
[0, 0, t_3 / 2 * alpha_ay_2, t_2 * alpha_ay_2]])
# ========== parameter setting ========== #
# print('interpolated_track_dict', interpolated_track_dict)
for track_id in interpolated_track_dict.keys():
track_ids = []
# print('track_id', track_id)
track_ids.append(track_id)
history_trajectory_x = interpolated_track_dict[track_id][0]
# print('history_trajectory_x',history_trajectory_x)
history_trajectory_y = interpolated_track_dict[track_id][1]
# print('history_trajectory_y', history_trajectory_y)
#x_in = np.mat([[history_trajectory_x[0], history_trajectory_y[0], 0.0, 0.0]]).transpose()
kf0 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf1 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf2 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf3 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf4 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf5 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf6 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf7 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
kf8 = KalmanFilter(x_in, P_in, H_in, R_in, Q_in)
for i in range(len(history_trajectory_x) - 1, -1, -1):
#===================== estimation done =================== #
pose_xy = np.mat(
[history_trajectory_x[i], history_trajectory_y[i]])
# print('pose_xy', pose_xy, i)
kf0.predict('CV', t)
kf0.update(pose_xy.transpose()) #need to transpose?
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf0.S_)
L_cv = gaussain_distribution.pdf(kf0.y_.transpose())
L_cv = max(1e-30, L_cv)
kf1.predict('CTA_a1', t)
kf1.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf1.S_)
L_cta_a1 = gaussain_distribution.pdf(kf1.y_.transpose())
L_cta_a1 = max(1e-30, L_cta_a1)
kf2.predict('CTA_a2', t)
kf2.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf2.S_)
L_cta_a2 = gaussain_distribution.pdf(kf2.y_.transpose())
L_cta_a2 = max(1e-30, L_cta_a2)
kf3.predict('CTA_d1', t)
kf3.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf3.S_)
L_cta_d1 = gaussain_distribution.pdf(kf3.y_.transpose())
L_cta_d1 = max(1e-30, L_cta_d1)
kf4.predict('CTA_d2', t)
kf4.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf4.S_)
L_cta_d2 = gaussain_distribution.pdf(kf4.y_.transpose())
L_cta_d2 = max(1e-30, L_cta_d2)
kf5.predict('CT_l1', t)
kf5.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf5.S_)
L_ct_l1 = gaussain_distribution.pdf(kf5.y_.transpose())
L_ct_l1 = max(1e-30, L_ct_l1)
kf6.predict('CT_l2', t)
kf6.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf6.S_)
L_ct_l2 = gaussain_distribution.pdf(kf6.y_.transpose())
L_ct_l2 = max(1e-30, L_ct_l2)
kf7.predict('CT_r1', t)
kf7.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf7.S_)
L_ct_r1 = gaussain_distribution.pdf(kf7.y_.transpose())
L_ct_r1 = max(1e-30, L_ct_r1)
kf8.predict('CT_r2', t)
kf8.update(pose_xy.transpose())
gaussain_distribution = scipy.stats.multivariate_normal([0, 0],
kf8.S_)
L_ct_r2 = gaussain_distribution.pdf(kf8.y_.transpose())
L_ct_r2 = max(1e-30, L_ct_r2)
prob_sum = self.AMM_prob_dict['CV'] * L_cv + \
self.AMM_prob_dict['CTA_a1'] * L_cta_a1 + self.AMM_prob_dict['CTA_a2'] * L_cta_a2 + \
self.AMM_prob_dict['CTA_d1'] * L_cta_d1 + self.AMM_prob_dict['CTA_d2'] * L_cta_d2 + \
self.AMM_prob_dict['CT_l1'] * L_ct_l1 + self.AMM_prob_dict['CT_l2'] * L_ct_l2 + \
self.AMM_prob_dict['CT_r1'] * L_ct_r1 + self.AMM_prob_dict['CT_r2'] * L_ct_r2
#print("+++++++++++++++++++++")
#print(self.AMM_prob_dict)
self.AMM_prob_dict[
'CV'] = self.AMM_prob_dict['CV'] * L_cv / prob_sum
self.AMM_prob_dict['CTA_a1'] = self.AMM_prob_dict[
'CTA_a1'] * L_cta_a1 / prob_sum
self.AMM_prob_dict['CTA_a2'] = self.AMM_prob_dict[
'CTA_a2'] * L_cta_a2 / prob_sum
self.AMM_prob_dict['CTA_d1'] = self.AMM_prob_dict[
'CTA_d1'] * L_cta_d1 / prob_sum
self.AMM_prob_dict['CTA_d2'] = self.AMM_prob_dict[
'CTA_d2'] * L_cta_d2 / prob_sum
self.AMM_prob_dict[
'CT_l1'] = self.AMM_prob_dict['CT_l1'] * L_ct_l1 / prob_sum
self.AMM_prob_dict[
'CT_l2'] = self.AMM_prob_dict['CT_l2'] * L_ct_l2 / prob_sum
self.AMM_prob_dict[
'CT_r1'] = self.AMM_prob_dict['CT_r1'] * L_ct_r1 / prob_sum
self.AMM_prob_dict[
'CT_r2'] = self.AMM_prob_dict['CT_r2'] * L_ct_r2 / prob_sum
current_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \
self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \
self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \
self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \
self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_
# print('current_x_estimation', current_x_estimation)
current_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_x_estimation - kf0.x_) * (current_x_estimation - kf0.x_).transpose()) + \
self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \
self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \
self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\
self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \
self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \
self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \
self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \
self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose())
#===================== estimation done =================== #
# now lets predict:
current_pose_xy = np.mat(
[history_trajectory_x[0], history_trajectory_y[0]])
# print('current_pose_xy',current_pose_xy)
kf0.predict('CV', self.period_of_predict)
kf1.predict('CTA_a1', self.period_of_predict)
kf2.predict('CTA_a2', self.period_of_predict)
kf3.predict('CTA_d1', self.period_of_predict)
kf4.predict('CTA_d2', self.period_of_predict)
kf5.predict('CT_l1', self.period_of_predict)
kf6.predict('CT_l2', self.period_of_predict)
kf7.predict('CT_r1', self.period_of_predict)
kf8.predict('CT_r2', self.period_of_predict)
prob_sum_p = self.AMM_prob_dict['CV'] + \
self.AMM_prob_dict['CTA_a1'] + self.AMM_prob_dict['CTA_a2'] + \
self.AMM_prob_dict['CTA_d1'] + self.AMM_prob_dict['CTA_d2'] + \
self.AMM_prob_dict['CT_l1'] + self.AMM_prob_dict['CT_l2'] + \
self.AMM_prob_dict['CT_r1'] + self.AMM_prob_dict['CT_r2'] + 0.00000000000001
self.AMM_prob_dict['CV'] = self.AMM_prob_dict['CV'] / prob_sum_p
self.AMM_prob_dict[
'CTA_a1'] = self.AMM_prob_dict['CTA_a1'] / prob_sum_p
self.AMM_prob_dict[
'CTA_a2'] = self.AMM_prob_dict['CTA_a2'] / prob_sum_p
self.AMM_prob_dict[
'CTA_d1'] = self.AMM_prob_dict['CTA_d1'] / prob_sum_p
self.AMM_prob_dict[
'CTA_d2'] = self.AMM_prob_dict['CTA_d2'] / prob_sum_p
self.AMM_prob_dict[
'CT_l1'] = self.AMM_prob_dict['CT_l1'] / prob_sum_p
self.AMM_prob_dict[
'CT_l2'] = self.AMM_prob_dict['CT_l2'] / prob_sum_p
self.AMM_prob_dict[
'CT_r1'] = self.AMM_prob_dict['CT_r1'] / prob_sum_p
self.AMM_prob_dict[
'CT_r2'] = self.AMM_prob_dict['CT_r2'] / prob_sum_p
predict_x_estimation = self.AMM_prob_dict['CV'] * kf0.x_ + \
self.AMM_prob_dict['CTA_a1'] * kf1.x_ + self.AMM_prob_dict['CTA_a2'] * kf2.x_ + \
self.AMM_prob_dict['CTA_d1'] * kf3.x_ + self.AMM_prob_dict['CTA_d2'] * kf4.x_ + \
self.AMM_prob_dict['CT_l1'] * kf5.x_ + self.AMM_prob_dict['CT_l2'] * kf6.x_ + \
self.AMM_prob_dict['CT_r1'] * kf7.x_ + self.AMM_prob_dict['CT_r2'] * kf8.x_
predict_P_estimation = self.AMM_prob_dict['CV'] * (kf0.P_ + (current_pose_xy - kf0.x_) * (current_pose_xy - kf0.x_).transpose()) + \
self.AMM_prob_dict['CTA_a1'] * (kf1.P_ + (current_x_estimation - kf1.x_) * (current_x_estimation - kf1.x_).transpose()) + \
self.AMM_prob_dict['CTA_a2'] * (kf2.P_ + (current_x_estimation - kf2.x_) * (current_x_estimation - kf2.x_).transpose()) + \
self.AMM_prob_dict['CTA_d1'] * (kf3.P_ + (current_x_estimation - kf3.x_) * (current_x_estimation - kf3.x_).transpose()) +\
self.AMM_prob_dict['CTA_d2'] * (kf4.P_ + (current_x_estimation - kf4.x_) * (current_x_estimation - kf4.x_).transpose()) + \
self.AMM_prob_dict['CT_l1'] * (kf5.P_ + (current_x_estimation - kf5.x_) * (current_x_estimation - kf5.x_).transpose()) + \
self.AMM_prob_dict['CT_l2'] * (kf6.P_ + (current_x_estimation - kf6.x_) * (current_x_estimation - kf6.x_).transpose()) + \
self.AMM_prob_dict['CT_r1'] * (kf7.P_ + (current_x_estimation - kf7.x_) * (current_x_estimation - kf7.x_).transpose()) + \
self.AMM_prob_dict['CT_r2'] * (kf8.P_ + (current_x_estimation - kf8.x_) * (current_x_estimation - kf8.x_).transpose())
self.predict_dict[track_id] = [
predict_x_estimation, predict_P_estimation
]
class FusionEKF:
def __init__(self):
self.kalman_filter = KalmanFilter()
self.is_initialized = False
self.previous_timestamp = 0
self.noise_ax = 1
self.noise_ay = 1
self.noise_az = 1
self.noise_vector = np.diag(
[self.noise_ax, self.noise_ay, self.noise_az])
self.kalman_filter.P = np.array([[1, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0,
0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
self.kalman_filter.x = np.zeros((6, 1))
# self.uwb_std = 23.5 / 1000
self.uwb_std = 23.5 / 10
self.R_UWB = np.array([[self.uwb_std**2]])
self.kalman_filter.R = self.R_UWB
def process_measurement(self, anchor_pose, anchor_distance):
if not self.is_initialized:
self.is_initialized = True
self.previous_timestamp = time.time()
return
current_time = time.time()
dt = current_time - self.previous_timestamp
self.previous_timestamp = current_time
self.calulate_F_matrix(dt)
self.calculate_Q_matrix(dt)
self.kalman_filter.predict()
self.kalman_filter.update(anchor_pose, anchor_distance)
# print("X:", self.kalman_filter.x)
# print("P:", self.kalman_filter.P)
# print(self.kalman_filter.Q)
def calulate_F_matrix(self, dt):
self.kalman_filter.F = np.array([[1, 0, 0, dt, 0, 0],
[0, 1, 0, 0, dt, 0],
[0, 0, 1, 0, 0,
dt], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]])
def calculate_Q_matrix(self, dt):
a = [[(dt**4) / 4, (dt**3) / 2], [(dt**3) / 2, dt**2]]
self.kalman_filter.Q = np.kron(a, self.noise_vector)
class EKF:
def __init__(self):
self.is_initialized = False
self.previous_timestamp = 0
# Initialize measurement covariance matrix - laser
self.R_laser = np.array([[0.0225, 0.0], [0.0, 0.0225]],
dtype=np.float32)
# Initialize measurement covariance matrix - radar
self.R_radar = np.array(
[[0.09, 0.0, 0.0], [0.0, 0.0009, 0.0], [0.0, 0.0, 0.09]],
dtype=np.float32)
# Initialize state variable
x_init = np.zeros(4, dtype=np.float32)
# Initialize state covariance matrix
P_init = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# System model - dt not yet taken into account
F_init = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]],
dtype=np.float32)
# Transformation from state variable to measurement
H_init = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], dtype=np.float32)
# Covariance matrix of process noise - dt not yet taken into account
Q_init = np.array([
[1, 0, 1, 0],
[0, 1, 0, 1],
[1, 0, 1, 0],
[0, 1, 0, 1],
],
dtype=np.float32)
# Initialize our Kalman filter
self.ekf = KalmanFilter(x_init, P_init, F_init, H_init, self.R_laser,
Q_init)
# Set the process noise constants
self.noise_ax = 9.0
self.noise_ay = 9.0
def process_measurement(self, m):
if not self.is_initialized:
# First measurement
if m['sensor_type'] == 'R':
# Convert radar data in polar to cartesian coordinates
# (Note that rho_dot from the very first measurement is
# not used))
px = m['rho'] * np.cos(m['phi'])
py = m['rho'] * np.sin(m['phi'])
self.ekf.x = np.array([px, py, 0.0, 0.0])
self.previous_timestamp = m['timestamp']
elif m['sensor_type'] == 'L':
# Initialize state variable
px, py = m['x'], m['y']
self.ekf.x = np.array([px, py, 0, 0])
self.previous_timestamp = m['timestamp']
self.is_initialized = True
return
# Prediction
# Update state transition matrix (time measured in seconds)
dt = (m['timestamp'] - self.previous_timestamp) / 1000000.0
self.previous_timestamp = m['timestamp']
self.ekf.F[0][2] = dt
self.ekf.F[1][3] = dt
# Set the process noise covariance
dt2 = dt * dt
dt3 = dt2 * dt
dt4 = dt3 * dt
self.ekf.Q = np.array(
[[dt4 * self.noise_ax / 4.0, 0.0, dt3 * self.noise_ax / 2.0, 0.0],
[0.0, dt4 * self.noise_ay / 4.0, 0.0, dt3 * self.noise_ay / 2.0],
[dt3 * self.noise_ax / 2.0, 0.0, dt2 * self.noise_ax, 0.0],
[0.0, dt3 * self.noise_ay / 2.0, 0.0, dt2 * self.noise_ay]],
dtype=np.float32)
self.ekf.predict()
# Measurement update
if m['sensor_type'] == 'R':
# Radar updates
z = np.array([m['rho'], m['phi'], m['rho_dot']], dtype=np.float32)
self.ekf.R = self.R_radar
self.ekf.update_ekf(z)
elif m['sensor_type'] == 'L':
# Laser updates
z = np.array([m['x'], m['y']], dtype=np.float32)
self.ekf.R = self.R_laser
self.ekf.update(z)
class KalmanBoxTracker(object):
"""
This class represents the internel state of individual tracked objects observed as bbox.
"""
count1 = 0
count2 = 0
def __init__(self, bbox, channel, reset_id):
"""
Initialises a tracker using initial bounding box.
"""
self.channel = channel
self.F = np.array(
[[1, 0, 0, 0, 1, 0, 0], [0, 1, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0, 1], [0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1]])
self.H = np.array(
[[1, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0, 0]])
self.kf = KalmanFilter(F=self.F, H=self.H)
self.kf.R[2:, 2:] *= 10.
self.kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities
self.kf.P *= 10.
self.kf.Q[-1, -1] *= 0.01
self.kf.Q[4:, 4:] *= 0.01
self.kf.x[:4], self.score, self.class_type = convert_bbox_to_z(bbox)
self.time_since_update = 0
if channel == 1:
if reset_id:
# print('channel1 is reset to 0')
KalmanBoxTracker.count1 = 0
self.id = KalmanBoxTracker.count1
KalmanBoxTracker.count1 += 1
elif channel == 2:
if reset_id:
# print('channel2 is reset to 0')
KalmanBoxTracker.count2 = 0
self.id = KalmanBoxTracker.count2
KalmanBoxTracker.count2 += 1
self.history = []
self.hits = 0
self.hit_streak = 0
self.age = 0
def update(self, bbox):
"""
Updates the state vector with observed bbox.
"""
self.time_since_update = 0
self.history = []
self.hits += 1
self.hit_streak += 1
self.kf.update(convert_bbox_to_z(bbox)[0])
def predict(self):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if (self.kf.x[6] + self.kf.x[2]) <= 0:
self.kf.x[6] *= 0.0
self.kf.predict()
self.age += 1
if self.time_since_update > 0:
self.hit_streak = 0
self.time_since_update += 1
self.history.append(convert_x_to_bbox(self.kf.x))
return self.history[-1]
def get_state(self):
"""
Returns the current bounding box estimate.
"""
return convert_x_to_bbox(self.kf.x)
def get_area(self):
if self.class_type == 0:
return 1.2
elif self.class_type == 1:
return 4.48
elif self.class_type == 2:
return 5.36
elif self.class_type == 3:
return 24.74
def get_velocity(self):
velocity_x = self.kf.x[4]
velocity_y = self.kf.x[5]
magnitude = int(np.sqrt(velocity_x ** 2 + velocity_y ** 2))
if (velocity_x != 0) & (velocity_y != 0):
direction = int(np.arctan(velocity_y / velocity_x) * (180 / np.pi))
return magnitude, direction
else:
return 0, 0
log.new_log('actual')
log.new_log('time')
log.new_log('covariance')
log.new_log('moving average')
# Moving average for measurements
moving_avg = MovingAverage(15)
# Number of iterations to perform
iterations = 100
for i in range(0, iterations):
# Generate a random acceleration
w = matrix(random.multivariate_normal([0.0, 0.0], Q)).T
# Predict
(X, P) = kf.predict(X, P, w)
# Update
(X, P) = kf.update(X, P, Z)
# Update the actual position
A = F * A + w
# Synthesise a new noisy measurement distributed around the real position
Z = matrix([[random.normal(A[0, 0], sigma_z)], [0.0]])
# Update the moving average with the latest measured position
moving_avg.update(Z[0, 0])
# Update the log for plotting later
log.log('measurement', Z[0, 0])
log.log('estimate', X[0, 0])
log.log('actual', A[0, 0])
log.log('time', i * dt)
log.log('covariance', P[0, 0])
log.log('moving average', moving_avg.getAvg())
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
def main():
R_values = [0.001, 0.1, 1, 10, 1000]
Q_values = [0.001, 0.1, 1, 10, 1000]
parser = argparse.ArgumentParser(
description="Run SSD on input folder and show result in popup window")
parser.add_argument("object_detector", choices=['ssd', 'yolo_full', 'yolo_tiny'],
help="Specify which object detector network should be used")
parser.add_argument("test", choices=['Q', 'R'],
help="Which noise matrix should be tested")
args = parser.parse_args()
if args.object_detector == 'ssd':
fd = SSDObjectDetection(frozen, pbtxt)
elif args.object_detector == 'yolo_full':
fd = YOLOObjectDetection('full')
elif args.object_detector == 'yolo_tiny':
fd = YOLOObjectDetection('tiny')
# Config
should_plot = False
# Get images list from dataset
dataset = "data/TinyTLP/"
for path, directories, files in os.walk(dataset):
all_directories = directories
break
results = {}
if args.test == 'R':
k = 2
Q_value = 1
for l, R_value in enumerate(R_values):
for dir in all_directories:
results[dir] = []
ground_truth_file = dataset + dir + "/groundtruth_rect.txt"
images_wildcard = dataset + dir + "/img/*.jpg"
images_filelist = glob(images_wildcard)
# Sort them in ascending order
# images_filelist = sorted(images_filelist, key=lambda xx: int(
# xx.split('/')[-1].split('.')[0]))
# Extract all ground truths
ground_truth = list(csv.reader(open(ground_truth_file)))
gt_measurements = []
for row in ground_truth:
gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)]))
initial_position = ground_truth[0]
frame_id, x, y, w, h, is_lost = initial_position
x = int(x)
y = int(y)
w = int(w)
h = int(h)
kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value)
# Iterate of every image
features = {}
t = tqdm(images_filelist[1:], desc="Processing")
da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1)
plt.ion()
for i, im in enumerate(t):
img = plt.imread(images_filelist[i])
height, width, _ = img.shape
# Compute features
features[i] = np.array(fd.compute_features(img))
# Do prediction
mu_bar, Sigma_bar = kf.predict()
# Do data association
da.update_prediction(mu_bar, Sigma_bar)
m = da.associate(features[i])
kf.update(m)
gt = list(map(int, ground_truth[i]))
kf_x = kf.get_x()
if should_plot:
x, y = np.mgrid[0:width, 0:height]
pos = np.empty(x.shape + (2,))
pos[:, :, 0] = x
pos[:, :, 1] = y
rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]])
f = rv.pdf(pos)
f[f < 1e-5] = np.nan
plt.gca()
plt.cla()
plt.imshow(img)
plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5)
if m is not None:
plt.plot(m[0], m[1], marker='o', color='yellow')
plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red')
plt.pause(0.0001)
print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}")
results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2]))
print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}")
with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp:
pickle.dump(results, fp)
elif args.test == 'Q':
l = 2
R_value = 1
for k, Q_value in enumerate(Q_values):
for dir in all_directories:
results[dir] = []
ground_truth_file = dataset + dir + "/groundtruth_rect.txt"
images_wildcard = dataset + dir + "/img/*.jpg"
images_filelist = glob(images_wildcard)
# Sort them in ascending order
# images_filelist = sorted(images_filelist, key=lambda xx: int(
# xx.split('/')[-1].split('.')[0]))
# Extract all ground truths
ground_truth = list(csv.reader(open(ground_truth_file)))
gt_measurements = []
for row in ground_truth:
gt_measurements.append(np.array([int(int(row[0]) - int(row[2]) / 2), int(int(row[1]) - int(row[3]) / 2)]))
initial_position = ground_truth[0]
frame_id, x, y, w, h, is_lost = initial_position
x = int(x)
y = int(y)
w = int(w)
h = int(h)
kf = KalmanFilter(x=np.array([[x + w / 2], [1], [y + h / 2], [1]]), Q=Q_value, R=R_value)
# Iterate of every image
features = {}
t = tqdm(images_filelist[1:], desc="Processing")
da = DataAssociation(R=kf.R, H=kf.H, threshold=0.1)
plt.ion()
for i, im in enumerate(t):
img = plt.imread(images_filelist[i])
height, width, _ = img.shape
# Compute features
features[i] = np.array(fd.compute_features(img))
# Do prediction
mu_bar, Sigma_bar = kf.predict()
# Do data association
da.update_prediction(mu_bar, Sigma_bar)
m = da.associate(features[i])
kf.update(m)
gt = list(map(int, ground_truth[i]))
kf_x = kf.get_x()
if should_plot:
x, y = np.mgrid[0:width, 0:height]
pos = np.empty(x.shape + (2,))
pos[:, :, 0] = x
pos[:, :, 1] = y
rv = multivariate_normal([kf.x[0][0], kf.x[2][0]], [[kf.cov[0][0], kf.cov[0][2]], [kf.cov[2][0], kf.cov[2][2]]])
f = rv.pdf(pos)
f[f < 1e-5] = np.nan
plt.gca()
plt.cla()
plt.imshow(img)
plt.contourf(x, y, f, cmap='coolwarm', alpha=0.5)
if m is not None:
plt.plot(m[0], m[1], marker='o', color='yellow')
plt.plot(gt[1] + w/2, gt[2] + h/2, marker='o', color='red')
plt.pause(0.0001)
print(f"Diff: {np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2])} Predicted position: {kf_x[0], kf_x[1]}, Ground truth position: {gt[1] + w / 2, gt[2] + h / 2}")
results[dir].append(np.linalg.norm([kf_x[0] - gt[1] - w / 2, kf_x[1] - gt[2] - h / 2]))
print(f"Dataset: {dir} mean distance: {np.mean(results[dir])}, std: {np.std(results[dir])}")
with open(f"results/kalman_filter_point_R_{l}_Q_{k}_{args.object_detector}.pickle", 'wb') as fp:
pickle.dump(results, fp)
red_rgb = (255, 0, 0)
# light_blue_rgb = (173,216,230)
draw_rectangle(rgb_frame, x1, y1, x2, y2, green_rgb, 1)
cx, cy = get_center_rect(x1, y1, x2, y2)
tracked_points.append([cx, cy])
if len(tracked_points) % nth_point_fade == 1 and len(
tracked_points) > nth_point_fade:
tracked_points.popleft()
for _point in tracked_points:
current_state_measurement = np.array([[_point[0]], [_point[1]]])
tracker_point = kf.predict()
_ = kf.correct(current_state_measurement)
# draw_circle(rgb_frame, _point[0], _point[1], 3, red_rgb)
draw_circle(rgb_frame, tracker_point[0], tracker_point[1], 3,
red_rgb)
bgr_frame = cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR)
cv2.imshow('frame', bgr_frame)
if cv2.waitKey(28) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
N_STEPS = 300
counter = 0
trajectory = []
for img_id in range(len(df['filename'].values)):
img_path = os.path.join(base_path, df['filename'].values[img_id])
img = cv2.imread(img_path, 0)
v, delta, current_time = df[['speed', 'angle', 'timestamp']].values[img_id]
#acc_idx = abs(ins['timestamp'].values - current_time).argmin()
#acc_x = ins['ax'].values[acc_idx]
#acc_y = -ins['ay'].values[acc_idx]
#print(v, delta)
dy_b, dx_b = model.step(v, delta, current_time)
kalman_y.predict(dy_b)
kalman_x.predict(-dx_b)
# Visual Odometery update
vo.update(img, img_id)
cur_t = vo.cur_t
if (img_id > 2):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
vo.x, vo.y, vo.z = x, y, z
lat, lon = df[['lat', 'long']].values[img_id]
d_xg, d_yg = latlon_to_xy_grid(lat, lon, prev_lat, prev_lon)
class Track:
def __init__(self, bb, id_, max_miss_):
self.track = [bb]
self.alive = True
self.kf = KalmanFilter()
self.id_track = id_
self.mean, self.cov = self.kf.initiate(bb.asp_ratio())
self.num_miss = 0
self.num_max_miss = max_miss_
self.project_mean = 0
self.color = (int(random.random() * 256), int(random.random() * 256),
int(random.random() * 256))
def last(self):
return self.track[-1]
def get_last_mean(self):
return self.mean[0:4]
def append(self, bb):
self.kalman_steps(bb.asp_ratio())
aproxBB = BoudingBox(coord=_restoreStandardValue(self.get_last_mean()),
frame=bb.getIDFrame())
self.track.append(aproxBB)
def kalman_steps(self, dets):
self.mean, self.cov = self.kf.predict(self.mean, self.cov)
self.mean, self.cov = self.kf.update(self.mean, self.cov, dets)
def getTrack(self):
return self.track
def getBBFrame(self, frame_id):
for bb in self.track:
if bb.getIDFrame() == frame_id:
return bb
def __len__(self):
return len(self.track)
def is_alive(self):
return self.alive
def ressurect(self):
self.alive = True
def project_position(self):
self.append(
BoudingBox(_restoreStandardValue(self.mean[:4]),
self.last().getIDFrame() + 1))
def missed(self):
self.num_miss += 1
self.project_position()
if self.num_miss > self.num_max_miss:
self.kill()
def kill(self):
self.alive = False
def getID(self):
return self.id_track
def getcolor(self):
return self.color
fig = plt.figure(figsize=plt.figaspect(1)) # Square figure
ax = fig.add_subplot(111, projection='3d')
# x_axis = np.arange(-20, 20, 0.1)
# plt.draw()
# plt.pause(0.1)
# input()
kf = KalmanFilter(dim=3, x=mu, P=sig, R=measurement_sig, Q=motion_sig)
## TODO: Loop through all measurements/motions
# this code assumes measurements and motions have the same length
# so their updates can be performed in pairs
for n in range(measurements.shape[0]):
# motion update, with uncertainty
mu, sig = kf.predict(Q=motion_sig)
print('Predict: {} \n{}\n\n'.format(mu, sig))
# measurement update, with uncertainty
mu, sig = kf.update(y=measurements[n], R=measurement_sig)
print('Update: {} \n{}\n\n'.format(mu, sig))
# print (mu)
measurement_sig -= np.eye(
3
) * 0.25 # Assumes noise reduces; keep this constant by commenting out this line if needed.
if PLOT:
plt.cla()
ax.axvline(c='grey', lw=1)
ax.axhline(c='grey', lw=1)
# g = []
# for x in x_axis:
for labeled_object in tracked_objects[track_id]:
z = labeled_object.bbox
print("z:")
print(z)
top_left_x = float(z[0])
top_left_y = float(z[1])
bottom_right_x = float(z[2])
bottom_right_y = float(z[3])
centre_x = (top_left_x + bottom_right_x) / 2
centre_y = (top_left_y + bottom_right_y) / 2
measurements = np.zeros((2, 1))
measurements[0] = centre_x
measurements[1] = centre_y
print("measurements:")
print(z)
x, P = filter.predict()
gt_points_x.append(measurements[0])
gt_points_y.append(measurements[1])
kalman_points_x.append(x[0])
kalman_points_y.append(x[1])
#print("x:")
print(labeled_object.bbox)
filter.update(measurements)
plt.scatter(gt_points_x, gt_points_y)
plt.scatter(kalman_points_x, kalman_points_y)
plt.show()
break
