def test_saver_kf():
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
s = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 10
assert len(s.y) == 10
assert len(s.K) == 10
assert (len(s) == len(s.x))
# Force an exception to occur my malforming K
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
kf.K = 0.
s2 = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s2.save()
# this should raise an exception because K is malformed
try:
s2.to_array()
assert False, "Should not have been able to convert s.K into an array"
except:
pass
def test_kinematic_filter():
global kf
# make sure the default matrices are shaped correctly
for dim_x in range(1, 4):
for order in range(0, 3):
kf = kinematic_kf(dim=dim_x, order=order)
kf.predict()
kf.update(np.zeros((dim_x, 1)))
# H is tricky, make sure it is shaped and assigned correctly
kf = kinematic_kf(dim=2, order=2)
assert kf.x.shape == (6, 1)
assert kf.F.shape == (6, 6)
assert kf.P.shape == (6, 6)
assert kf.Q.shape == (6, 6)
assert kf.R.shape == (2, 2)
assert kf.H.shape == (2, 6)
H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]], dtype=float)
assert np.array_equal(H, kf.H)
kf = kinematic_kf(dim=3, order=2, order_by_dim=False)
H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0]],
dtype=float)
assert np.array_equal(H, kf.H)
def test_kinematic_filter():
global kf
# make sure the default matrices are shaped correctly
for dim_x in range(1,4):
for order in range (0, 3):
kf = kinematic_kf(dim=dim_x, order=order)
kf.predict()
kf.update(np.zeros((dim_x, 1)))
# H is tricky, make sure it is shaped and assigned correctly
kf = kinematic_kf(dim=2, order=2)
assert kf.x.shape == (6, 1)
assert kf.F.shape == (6, 6)
assert kf.P.shape == (6, 6)
assert kf.Q.shape == (6, 6)
assert kf.R.shape == (2, 2)
assert kf.H.shape == (2, 6)
H = np.array([[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]], dtype=float)
assert np.array_equal(H, kf.H)
kf = kinematic_kf(dim=3, order=2, order_by_dim=False)
H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0]], dtype=float)
assert np.array_equal(H, kf.H)
def test_saver_kf():
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
s = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 10
assert len(s.y) == 10
assert len(s.K) == 10
assert (len(s) == len(s.x))
# Force an exception to occur my malforming K
kf = kinematic_kf(3, 2, dt=0.1, dim_z=3)
kf.K = 0.
s2 = Saver(kf)
for i in range(10):
kf.predict()
kf.update([i, i, i])
s2.save()
# this should raise an exception because K is malformed
try:
s2.to_array()
assert False, "Should not have been able to convert s.K into an array"
except:
pass
def setUp(self):
self.kinematic_test_kf = kinematic_kf(dim=1,order=1,dt=1)
x = [0.,2.]
self.zs = [x[0]]
self.pos = [x[0]]
noise_std = 1.
# Noisy measurements generation for 30 samples
for _ in range(30):
last_pos = x[0]
last_vel = x[1]
new_vel = last_vel
new_pos = last_pos + last_vel
x = [new_pos, new_vel]
z = new_pos + (randn()*noise_std)
self.zs.append(z)
# Outlier generation
self.zs[5] += 10.
self.zs[10] += 10.
self.zs[15] += 10.
self.zs[20] += 10.
self.zs[25] += 10.
self.detector = MahalanobisDetector()
def __init__(self, dim, dim_z):
self.kf = kinematic_kf(dim, 1, dim_z=dim_z, order_by_dim=False,)
self.id = self.__class__.n_trackers
self.__class__.n_trackers += 1
self.time_since_update = 0
self.age = 0
self.hits = 0
self.hit_streak = 0
def test_kinematic_filter():
# make sure the default matrices are shaped correctly
for dim_z in range(1, 4):
for dim_x in range(1, 4):
for order in range(0, 3):
kf = kinematic_kf(dim=dim_x, order=order, dim_z=dim_z)
kf.predict()
kf.update(np.zeros((dim_z, 1)))
kf = kinematic_kf(dim=dim_x,
order=order,
dim_z=dim_z,
order_by_dim=False)
kf.predict()
kf.update(np.zeros((dim_z, 1)))
# H is tricky, make sure it is shaped and assigned correctly
kf = kinematic_kf(dim=2, order=2)
assert kf.x.shape == (6, 1)
assert kf.F.shape == (6, 6)
assert kf.P.shape == (6, 6)
assert kf.Q.shape == (6, 6)
assert kf.R.shape == (1, 1)
assert kf.H.shape == (1, 6)
H = np.array([[1., 0, 0, 1, 0, 0]])
assert np.array_equal(H, kf.H), "H is {}, should be {}".format(kf.H, H)
kf = kinematic_kf(dim=2, order=2, order_by_dim=False)
assert kf.x.shape == (6, 1)
assert kf.F.shape == (6, 6)
assert kf.P.shape == (6, 6)
assert kf.Q.shape == (6, 6)
assert kf.R.shape == (1, 1)
assert kf.H.shape == (1, 6)
H = np.array([[1., 1, 0, 0, 0, 0]])
assert np.array_equal(H, kf.H), "H is\n{}\nshould be\n{}".format(kf.H, H)
kf = kinematic_kf(dim=1, order=2, dim_z=3, order_by_dim=False)
H = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0.]])
assert np.array_equal(H, kf.H), "H is\n{}\nshould be\n{}".format(kf.H, H)
def __init__(self, n_bodyparts):
# TODO Try particle filter (since we already have the keypoints)
self.kf = kinematic_kf(
n_bodyparts * 2, order=1, dim_z=n_bodyparts, order_by_dim=False
)
self.kf.Q[self.kf.dim_z :, self.kf.dim_z :] *= 10
self.kf.R[self.kf.dim_z :, self.kf.dim_z :] *= 0.01
self.kf.P[self.kf.dim_z :, self.kf.dim_z :] *= 1000
self.id = SkeletonTracker.n_trackers
SkeletonTracker.n_trackers += 1
self.time_since_update = 0
self.age = 0
self.hits = 0
self.hit_streak = 0
def test_steadystate():
dim = 7
cv = kinematic_kf(dim=dim, order=5)
cv.x[1] = 1.0
for i in range(100):
cv.predict()
cv.update([i] * dim)
for i in range(100):
cv.predict_steadystate()
cv.update_steadystate([i] * dim)
def __init__(self, params):
self.kf = kinematic_kf(5, order=1, dim_z=5, order_by_dim=False)
self.kf.R[2:, 2:] *= 10.0
self.kf.P[
5:, 5:
] *= 1000.0 # High uncertainty to the unobservable initial velocities
self.kf.P *= 10.0
self.kf.Q[5:, 5:] *= 0.01
self.state = params
self.time_since_update = 0
self.id = EllipseTracker.n_trackers
EllipseTracker.n_trackers += 1
self.hits = 0
self.hit_streak = 0
self.age = 0
def __init__(self, trackId):
super(BeeTrack, self).__init__()
##! Name that gets shown on the screen for that bee
self._name = ""
## Last detected bee position
self._last_dectect = None
## The tracks ID
self.trackId = trackId
# Initialize the klaman filter
self.dt = 1
self.KF = kinematic_kf(dim=2,
order=2,
dt=self.dt,
dim_z=1,
order_by_dim=True)
self.KF.R *= 2
self.KF.Q = np.array(
[[self.dt**4 / 4, self.dt**3 / 2, self.dt**4 / 2, 0, 0, 0],
[self.dt**3 / 2, self.dt**2, self.dt**4, 0, 0, 0],
[self.dt**3 / 1, self.dt**1, self.dt**1 / 2, 0, 0, 0],
[0, 0, 0, self.dt**4 / 4, self.dt**3 / 2, self.dt**4 / 2],
[0, 0, 0, self.dt**3 / 2, self.dt**2, self.dt**4],
[0, 0, 0, self.dt**3 / 1, self.dt**1, self.dt**1 / 2]])
# Keep track of the
self.trace = deque(maxlen=get_config("MAX_BEE_TRACE_LENGTH"))
# Amount of missed detetions
self.skipped_frames = 0
# Amount of frames processed with this track
self.processed_frames = 0
# A set if determined track/bee characteristics
self.tags = set()
self.reported_tags = set()
# The first detection which created this track
self.first_position = None
# Whether the track is underneath of a group of bees
self.in_group = False
self.__tagCnts = {}
def test_steadystate():
dim = 7
cv = kinematic_kf(dim=dim, order=5)
cv.x[1] = 1.0
for i in range(100):
cv.predict()
cv.update([i]*dim)
for i in range(100):
cv.predict_steadystate()
cv.update_steadystate([i]*dim)
# test mahalanobis
a = np.zeros(cv.y.shape)
maha = scipy_mahalanobis(a, cv.y, cv.SI)
assert cv.mahalanobis == approx(maha)
def rtsmooth(measurements, dt=0.02, order=2):
"""
Args:
measurements (np.array): (time, measurements_dim)
Returns:
data (np.array): (time, measurements_dim)
"""
measure_dim = measurements.shape[1]
kf = kinematic_kf(dim=measure_dim, order=order, dt=dt)
# print(kf.F[:3, :3]) # State transition
# kf.P is ordered with [2, 2] or [3, 3] blocks for each dimension
# (2 if 1st order - constant velocity, 3 if 2nd order - constant acceleration)
kf.P[::order + 1, ::order + 1] *= 1
kf.P *= 10
kf.Q[::order + 1, ::order + 1] *= 1
mu, cov, _, _ = kf.batch_filter(npt.numpify(measurements))
smoothed, _, _, _ = kf.rts_smoother(mu, cov)
print(smoothed.shape)
return smoothed[:, ::order + 1, 0]
def test_steadystate():
dim = 7
cv = kinematic_kf(dim=dim, order=5)
cv.x[1] = 1.0
for i in range(100):
cv.predict()
cv.update([i] * dim)
for i in range(100):
cv.predict_steadystate()
cv.update_steadystate([i] * dim)
# test mahalanobis
a = np.zeros(cv.y.shape)
maha = scipy_mahalanobis(a, cv.y, cv.SI)
assert cv.mahalanobis == approx(maha)
def test_kinematic():
kf = kinematic_kf(1,1)
import numpy as np
from filterpy.common import kinematic_kf
kf1 = kinematic_kf(2, 2)
kf2 = kinematic_kf(2, 2)
# do some settings of x, R, P etc. here, I'll just use the defaults
kf2.Q *= 0 # no prediction error in second filter
filters = [kf1, kf2]
mu = [0.5, 0.5] # each filter is equally likely at the start
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
imm = IMMEstimator(filters, mu, trans)
for i in range(100):
# make some noisy data
x = i + np.random.randn() * np.sqrt(kf1.R[0, 0])
y = i + np.random.randn() * np.sqrt(kf1.R[1, 1])
z = np.array([[x], [y]])
# perform predict/update cycle
imm.predict()
imm.update(z)
print(imm.x.T)
accel_magnitudes = np.ndarray.tolist((process.get_column(name='accel_mag')))
timesteps = np.ndarray.tolist((process.get_column(index=0)))
data = np.array([process.get_column(index=0), process.get_column(index=11), process.get_column(name='accel_mag')])
data = data.T
print(data)
#print(process.get_column(index=0))
# todo use numpy
alt_list = []
vel_list = []
kf = kinematic_kf(dim=1, order=3, dt=.04)
transition_matrix = kf.F
print(kf.F)
# init altitude
kf.x[0] = data[0][1]
print('init', data[0][1])
kst = kinematic_state_transition(3, dt=.04)
kinematics = np.empty(np.append(0, kf.x).shape)
#kinematics.reshape(4, 1)
print(kinematics)
counter = 0
for time, alt, accel_mag in data:
def test_kinematic():
kf = kinematic_kf(1, 1)
#!/usr/bin/env python
import time
import math
import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped
import numpy as np
from filterpy.common import Q_discrete_white_noise, kinematic_kf
launchTime = rospy.get_param("/launch_time")
launchTime += 5.0
freq = 20.0
latestSVGS = PoseStamped()
f = kinematic_kf(3, 1, dt=(1.0 / freq), order_by_dim=False)
def kalmanFilter(msg):
global latestSVGS, latestVel, latestAcc, t0, t1, f
latestSVGS = msg
x = latestSVGS.pose.position.y
y = latestSVGS.pose.position.z
z = latestSVGS.pose.position.x
z = np.array([x, y, z])
f.predict()
f.update(z)
# print (f.x)
positionEstimate = PoseStamped()
velocityEstimate = TwistStamped()
from filterpy.common import kinematic_kf
from filterpy.kalman import ExtendedKalmanFilter
import time
import numpy as np
import filterpy as fp
from filterpy.kalman import IMMEstimator
np.set_printoptions(suppress=True)
order = 1
kf1 = kinematic_kf(dim=2, order=order)
kf2 = kinematic_kf(dim=2, order=order)
kf2.Q *= 0 # no prediction error in second filter
filters = [kf1, kf2]
mu = [0.5, 0.5] # filter probability
trans = np.array([[0.97, 0.03], [0.03, 0.97]])
imm = IMMEstimator(filters, mu, trans)
def aim(bounding_boxes):
# TODO: Needs testing
# TODO: Finalize on input contract
if len(bounding_boxes) == 0:
return imm.x_prior[0], imm.x_prior[2] # return last result
if isinstance(bounding_boxes[0], list):
x, y, w, h = bounding_boxes[0]
else:
x, y, w, h = bounding_boxes
xc = x + int(w / 2)
yc = y + int(h / 2)
Parameters
----------
measurement: float numpy array
Measurement coming from a radar and needed to be tested.
self.filter: KalmanFilter object
The Kalman filter the detector is attached to.
'''
test_quantity = sqrt(filter.y.T @ filter.y)
return test_quantity
if __name__ == "__main__":
# Example Kalman filter for a kinematic model
kinematic_test_kf = kinematic_kf(dim=1, order=1, dt=1)
x = [0., 2.]
zs = [x[0]]
pos = [x[0]]
noise_std = 1.
# Noisy position measurements generation for 30 samples
for _ in range(30):
last_pos = x[0]
last_vel = x[1]
new_vel = last_vel
new_pos = last_pos + last_vel
x = [new_pos, new_vel]
z = new_pos + (randn() * noise_std)
zs.append(z)
pos.append(new_pos)
