def one_dim_kalman_filter(z_seq,
measure_std_seq,
proc_std,
dim_x=3,
x0=None,
P0=None,
dt=1):
# NOTE: we do not specify velocity and acceleration in predict function,
# since the filter will track it
assert dim_x == 2 or dim_x == 3
kf = KalmanFilter(dim_x=dim_x, dim_z=1)
if dim_x == 2:
# x is the state vector
if x0 is None:
kf.x = np.array([z_seq[0], 0])
else:
assert x0.shape[0] == dim_x
kf.x = x0
# P is the covariance matrix of state
if P0 is None:
kf.P = np.array([[1, 0], [0, 1]])
else:
assert P0.shape[0] == dim_x and P0.shape[1] == dim_x
kf.P = P0
# F is the state transmition function
kf.F = np.array([[1, dt], [0, 1]])
# H is the measurement function
kf.H = np.array([[1, 0]])
else:
if x0 is None:
kf.x = np.array([z_seq[0], 0, 0])
else:
assert x0.shape[0] == dim_x
kf.x = x0
if P0 is None:
kf.P = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
else:
assert P0.shape[0] == dim_x and P0.shape[1] == dim_x
kf.P = P0
kf.F = np.array([[1, dt, 0.5 * dt**2], [0, 1, dt], [0, 0, 1]])
kf.H = np.array([[1, 0, 0]])
# Q is the covariance matrix of process noise
kf.Q = Q_discrete_white_noise(dim=dim_x, dt=dt, var=proc_std**2)
# TODO: change the scale of R??
std_scale = 1
R_seq = (measure_std_seq * std_scale)**2
x_seq, cov_seq = [kf.x], [kf.P]
# z is the measurement vector
# R is the covariance matrix of measurement noise
for z, R in zip(z_seq[1:], R_seq[1:]):
kf.predict()
kf.update(z, R=np.array([[R]]))
x_seq.append(kf.x)
cov_seq.append(kf.P)
x_seq, cov_seq = np.array(x_seq), np.array(cov_seq)
return x_seq[:, 0]
def tracker1():
#
#
# Design 2D filter
#
#
# 1. Choose state vars - x
# 2. Design state trans. Function - F
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.
# time step 1 second
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
# 3. Design Process Noise Mat - Q
v = 0.05
q = Q_discrete_white_noise(dim=2, dt=dt, var=v)
tracker.Q = block_diag(q, q)
# 4. B
# 5. Design measurement function
tracker.H = np.array([[1/0.3048, 0, 0, 0],
[0, 0, 1/0.3048, 0]])
# 6. Design Meas. Noise Mat - R
tracker.R = np.array([[5, 0],[0, 5]])
# Init conditions
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
def kf(self, x, dt):
'''
dt is time difference betweeen two frames
'''
kf = KalmanFilter(dim_x=4, dim_z=2)
kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
kf.u = 0.
kf.H = np.array([
[
1, 0, 0, 0
], # for initialization of R we should take the measurement errors for position
[0, 0, 1, 0]
])
kf.R = np.diag(
[0.01, 0.01]
) # for initialization of R we should take the measurement errors for position
vel_max = 10 # maximum velocity of a human
q = Q_discrete_white_noise(dim=2, dt=dt,
var=0.01) # here var is changable
kf.Q = block_diag(q, q)
kf.x = x
variance_of_vel = np.diag([vel_max**2, vel_max**2])
kf.P = block_diag(
kf.R[0, 0], variance_of_vel[0, 0], kf.R[1, 1], variance_of_vel[
1, 1]) #initialize P with R and square of maximum velocity
return kf
def kfilter(self):
tracker = KalmanFilter(dim_x=8, dim_z=8)
dt = 1.0 # time step
d3 = 0.0 #d-triangle, correlation between VXs
dc = 0.0 #d-circle, correlation between VYs
a = 1
#cx1=1,cx2=0.5,cx3=0.25
tracker.F = np.array([ #x1 vx y1 vy x2 vx2 y2 vy2
[1, dt, 0, 0, a, 0, 0, 0], [0, 1, 0, 0, 0, a, 0, 0],
[0, 0, 1, dt, 0, 0, a, 0], [0, 0, 0, 1, 0, 0, 0, a],
[0, 0, 0, 0, 1, dt, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 0, 0, 1]
])
tracker.u = 0.
tracker.H = np.array([[1, 0, 0, 0, 0, 0, 0,
0], [0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0,
0], [0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0,
0], [0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 1]])
tracker.R = np.eye(8) * self.R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=self.Q_std**2)
tracker.Q = block_diag(q, q) #tracker.Q.dim = 4
tracker.Q = block_diag(tracker.Q, tracker.Q) #tracker.Q.dim = 8
v = 0.00001
tracker.x = np.array([[0, v, 0, v, 0, v, 0, v]]).T
tracker.P = np.eye(
8
) * 225. # (3*5)^2, 25,35,75,85 araliginda 5er li dagilim in 3 sigma variansi.
return tracker
def initKalman(self, X, Y, dt_average):
'''
Description :
Input :
Output :
'''
k_filter = KalmanFilter(dim_x=4, dim_z=2)
dt = dt_average #time step in seconds
k_filter.x = np.array([X[0], X[1] - X[0], Y[0], Y[1] - Y[0]
]).T # initial state (x and y position)
k_filter.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]]) # state transition matrix
q = Q_discrete_white_noise(dim=2, dt=dt,
var=np.cov(X,
Y)[0][0]) # process uncertainty
k_filter.Q = block_diag(q, q)
k_filter.H = np.array([[1, 0, 0, 0], [0, 0, 1,
0]]) # Measurement function
k_filter.R = np.cov(X, Y) # state uncertainty
P = np.eye(4)
covar = np.cov(X, Y)
P[0][0] = covar[0][0]
P[1][1] = covar[1][0]
P[2][2] = covar[0][1]
P[3][3] = covar[1][1]
k_filter.P = P # covariance matrix
return k_filter
def ball_filter6(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=6)
g = 10
f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0],
[0, 1., dt, 0, 0, 0],
[0, 0, 1., 0, 0, 0],
[0, 0, 0., 1., dt, -0.5*dt*dt*g],
[0, 0, 0, 0, 1., -g*dt],
[0, 0, 0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0],
[0,0,0,1,0,0],
[0,0,0,0,0,0],
[0,0,0,0,0,0]])
f1.R = np.mat(np.eye(6)) * R
f1.Q = np.zeros((6,6))
f1.Q[2,2] = Q
f1.Q[5,5] = Q
f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T
f1.P = np.eye(6) * 50.
f1.B = 0.
f1.u = 0
return f1
def test_rts():
fk = KalmanFilter(dim_x=2, dim_z=1)
fk.x = np.array([-1., 1.]) # initial state (location and velocity)
fk.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix
fk.H = np.array([[1., 0.]]) # Measurement function
fk.P = .01 # covariance matrix
fk.R = 5 # state uncertainty
fk.Q = 0.001 # process uncertainty
zs = [t + random.randn() * 4 for t in range(40)]
mu, cov, _, _ = fk.batch_filter(zs)
mus = [x[0] for x in mu]
M, P, _, _ = fk.rts_smoother(mu, cov)
if DO_PLOT:
p1, = plt.plot(zs, 'cyan', alpha=0.5)
p2, = plt.plot(M[:, 0], c='b')
p3, = plt.plot(mus, c='r')
p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["measurement", "RKS", "KF output", "ideal"],
loc=4)
plt.show()
def createTracker(iniVector):
"""
Creates the Kalman filter matrices to track and predict the Thymio's trajectory
:param iniVector: initial position and velocity state of the robot
:return: object tracker of class KalmanFilter with all Kalman matrices
"""
tracker = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1 # time step
tracker.F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]])
tracker.B = np.array([[dt, 0],
[0, dt],
[1, 0],
[0, 1]])
tracker.u = np.array([[1.],[1.]])
tracker.H = np.diag([1.,1.,1.,1.])
R = np.diag([5.,5.,3.,3.])
tracker.Q = np.diag([1.3,1.3,3.5,3.5])
tracker.x = iniVector
tracker.P = np.diag([1,1,1,1])
return tracker
def template():
kf = KalmanFilter(dim_x=2, dim_z=1)
# x0
kf.x = np.array([[.0], [.0]])
# P - change over time
kf.P = np.diag([500., 49.])
# F - state transition matrix
dt = .1
kf.F = np.array([[1, dt], [0, 1]])
## now can predict
## дисперсия растет и становится видна корреляция
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='r')
#kf.predict()
#plot_covariance_ellipse(kf.x, kf.P, edgecolor='k', ls='dashed')
#show()
# Control information
kf.B = 0
# !!Attention!! Q! Process noise
kf.Q = Q_discrete_white_noise( dim=2, dt=dt, var=2.35)
# H - measurement function
kf.H = np.array([[1., 0.]])
# R - measure noise matrix
kf.R = np.array([[5.]])
def ball_filter4(dt,R=1., Q = 0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat ([[1., dt, 0, 0,],
[0, 1., 0, 0],
[0, 0, 1., dt],
[0, 0, 0., 1.]])
f1.H = np.mat([[1,0,0,0],
[0,0,0,0],
[0,0,1,0],
[0,0,0,0]])
f1.B = np.mat([[0,0,0,0],
[0,0,0,0],
[0,0,1.,0],
[0,0,0,1.]])
f1.u = np.mat([[0],
[0],
[-0.5*g*dt**2],
[-g*dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4,4))
f1.Q[1,1] = Q
f1.Q[3,3] = Q
f1.x = np.mat([0, 0 , 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def test_rts():
fk = KalmanFilter(dim_x=2, dim_z=1)
fk.x = np.array([-1., 1.]) # initial state (location and velocity)
fk.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
fk.H = np.array([[1.,0.]]) # Measurement function
fk.P = .01 # covariance matrix
fk.R = 5 # state uncertainty
fk.Q = 0.001 # process uncertainty
zs = [t + random.randn()*4 for t in range(40)]
mu, cov, _, _ = fk.batch_filter (zs)
mus = [x[0] for x in mu]
M, P, _, _ = fk.rts_smoother(mu, cov)
if DO_PLOT:
p1, = plt.plot(zs,'cyan', alpha=0.5)
p2, = plt.plot(M[:,0],c='b')
p3, = plt.plot(mus,c='r')
p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["measurement", "RKS", "KF output", "ideal"], loc=4)
plt.show()
def pva_kalman_filter():
kf = KalmanFilter(dim_x=3, dim_z=1)
# Time Step
dt = 0.1
# Control Inputs
kf.u = 0
# Measurement Function (we are only recording position)
kf.H = np.array([[1, 0, 0]])
# State Variable
kf.x = np.array([[0, 0, 0]]).T
# Measurement Noise
kf.R = 20
# Process/Motion Noise
kf.Q = np.eye(3) * .1
# Covariance Matrix
kf.P = np.eye(3) * 1
# State Transition Function
kf.F = np.array([[1., dt, .5 * dt * dt], [0., 1., dt], [0., 0., 1.]])
## --------------- PREDICT / UPDATE -----------##
for z in mag_compass:
kf.predict()
kf.update(z)
pys.append(kf.x[0])
def xy_of_filter_init(self):
'''XY Filter'''
KFXY = KalmanFilter(dim_x=4, dim_z=2, dim_u=1) # Set up the XY filter
KFXY.x = np.array(
[
[0], #dx(pitch)
[0], #dy(roll)
[0], #vx(pitch)
[0]
], #vy(roll)
dtype=float)
KFXY.F = np.diag([1., 1., 1., 1.])
KFXY.P = np.diag([.9, .9, 1., 1.])
KFXY.B = np.diag([1., 1., 1., 1.])
KFXY.H = np.array([[0, 0, 1., 0], [0, 0, 0, 1.]])
KFXY.Q *= 0.1**2
KFXY.R *= 0.01**2
KFXY_z = np.array(
[
[0.], # Update value of the XY filter
[0.]
],
dtype=float)
KFXY_u = np.array(
[
[0.], # Control input for XY filter
[0.],
[0.],
[0.]
],
dtype=float)
return KFXY, KFXY_z, KFXY_u # Filter, Filter_Sensor_varible, Control_input
def createLegKF(x, y):
# kalman_filter = KalmanFilter(dim_x=4, dim_z=2)
kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
KF_F = np.array([[1.0, dt, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, dt], [0, 0, 0, 1.0]])
KF_q = 0.7 # 0.3
KF_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q), np.zeros((2, 2)))),
np.hstack((np.zeros((2, 2)), Q_discrete_white_noise(2, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_P = np.diag([KF_pd, KF_pv, KF_pd, KF_pv])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
# KF_R = np.diag([KF_rd,KF_rd])
KF_R = np.diag([KF_rd, KF_rd, KF_rv, KF_rv])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
KF_H = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0], [0, 1.0, 0, 0], [0, 0, 0, 1.0]])
kalman_filter.x = np.array([x, 0, y, 0])
kalman_filter.F = KF_F
kalman_filter.H = KF_H
kalman_filter.Q = KF_Q
kalman_filter.B = 0
kalman_filter.R = KF_R
kalman_filter.P = KF_P
return kalman_filter
def init_filter(dt):
# Establish Kalman Filter with 4 dimensions (x, xdot, y, ydot) and 2 measurements (x, y)
f = KalmanFilter(dim_x=4, dim_z=2)
# set state transition matrix (our best linear model for the system)
f.F = np.array([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.],
[0., 0., 0., 1.]])
# set control transition matrix
f.B = np.array([[0], [-.5 * pow(dt, 2)], [0], [-dt]])
# define the measurement function (what our measurement, z measures)
f.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
# define initial covariance. We are not confident in our initial state, so we make it relatively high
# The initial position x,y have a much lower variance then the initial velocity
# no covariance between x and y directions
sigma_sq_cov = 0.1
f.P = sigma_sq_cov * np.array([[.1, 0, 0.4472, 0], [0, .1, 0, 0.4472],
[0.4472, 0, 2, 0], [0, 0.4472, 0, 2]])
# define process covariance matrix (covariance of the error in our model)
# Set covariance between x and y dimensional variables to 0. Calculate other covariances as normal
f.Q = Q_discrete_white_noise(dim=4, dt=dt, var=0.1)
# define measurement covariance matrix (covariance of the error in our measurement)
sigma_sq_measurement = .0001
f.R = sigma_sq_measurement * np.array([[1, 0], [0, 1]])
return f
def get_linear_tracker():
# KF related
dt = IMSHOW_SLEEP_TIME / 1000 # time step
R_std = 0.35
Q_std = 0.04
M_TO_FT = 1 / 0.3048
tracker = KalmanFilter(dim_x=4, dim_z=2)
tracker.F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0],
[0, 0, 0, 1]])
tracker.H = np.array([[M_TO_FT, 0, 0, 0], [0, M_TO_FT, 0, 0]])
tracker.R = np.eye(2) * R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
tracker.Q[0, 0] = q[0, 0]
tracker.Q[1, 1] = q[0, 0]
tracker.Q[2, 2] = q[1, 1]
tracker.Q[3, 3] = q[1, 1]
tracker.Q[0, 2] = q[0, 1]
tracker.Q[2, 0] = q[0, 1]
tracker.Q[1, 3] = q[0, 1]
tracker.Q[3, 1] = q[0, 1]
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
def test_procedural_batch_filter():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2., 0])
f.F = np.array([[1., 1.], [0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.eye(2) * 1000.
f.R = np.eye(1) * 5
f.Q = Q_discrete_white_noise(2, 1., 0.0001)
f.test_matrix_dimensions()
x = np.array([2., 0])
F = np.array([[1., 1.], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2) * 1000.
R = np.eye(1) * 5
Q = Q_discrete_white_noise(2, 1., 0.0001)
zs = [13., None, 1., 2.] * 10
m, c, _, _ = f.batch_filter(zs, update_first=False)
n = len(zs)
mp, cp, _, _ = batch_filter(x, P, zs, [F] * n, [Q] * n, [H] * n, [R] * n)
for x1, x2 in zip(m, mp):
assert np.allclose(x1, x2)
for p1, p2 in zip(c, cp):
assert np.allclose(p1, p2)
def ball_filter4(dt, R=1., Q=0.1):
f1 = KalmanFilter(dim=4)
g = 10
f1.F = np.mat([[
1.,
dt,
0,
0,
], [0, 1., 0, 0], [0, 0, 1., dt], [0, 0, 0., 1.]])
f1.H = np.mat([[1, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0]])
f1.B = np.mat([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0], [0, 0, 0, 1.]])
f1.u = np.mat([[0], [0], [-0.5 * g * dt**2], [-g * dt]])
f1.R = np.mat(np.eye(4)) * R
f1.Q = np.zeros((4, 4))
f1.Q[1, 1] = Q
f1.Q[3, 3] = Q
f1.x = np.mat([0, 0, 0, 0]).T
f1.P = np.eye(4) * 50.
return f1
def kalman_filter_clean_algorithm(iq_mat):
plt.figure()
plt.imshow(iq_mat)
plt.savefig("before_kalman_filter")
Filter = KalmanFilter(dim_x=iq_mat.shape[0], dim_z=iq_mat.shape[0])
Filter.x = np.copy(iq_mat[:, 0])
Filter.H = np.eye(iq_mat.shape[0])
Filter.P = np.eye(iq_mat.shape[0]) * 0.1
Filter.Q = np.eye(iq_mat.shape[0]) * 0.1
for slow_axis_index in range(1, iq_mat.shape[1]):
plt.figure()
plt.plot(Filter.x)
plt.savefig("kalman_filter_estimation")
Filter.predict()
Filter.update(z=np.copy(iq_mat[:, slow_axis_index]))
for slow_axis_index in range(0, iq_mat.shape[1]):
iq_mat[:, slow_axis_index] = iq_mat[:, slow_axis_index] - Filter.x
plt.figure()
plt.imshow(iq_mat)
plt.savefig("after_kalman_filter")
return iq_mat
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.0], [0.0]]) # initial state (location and velocity)
f.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
f.H = np.array([[1.0, 0.0]]) # Measurement function
f.P *= 1000.0 # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
fsq = SquareRootKalmanFilter(dim_x=2, dim_z=1)
fsq.x = np.array([[2.0], [0.0]]) # initial state (location and velocity)
fsq.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
fsq.H = np.array([[1.0, 0.0]]) # Measurement function
fsq.P = np.eye(2) * 1000.0 # covariance matrix
fsq.R = 5 # state uncertainty
fsq.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn() * 20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
fsq.update(z)
fsq.predict()
assert abs(f.x[0, 0] - fsq.x[0, 0]) < 1.0e-12
assert abs(f.x[1, 0] - fsq.x[1, 0]) < 1.0e-12
# save data
results.append(f.x[0, 0])
measurements.append(z)
p = f.P - fsq.P
print(f.P)
print(fsq.P)
for i in range(f.P.shape[0]):
assert abs(f.P[i, i] - fsq.P[i, i]) < 0.01
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2.0, 0]]).T
f.P = np.eye(2) * 100.0
m, c, _, _ = f.batch_filter(zs, update_first=False)
def test_1d_0P():
global inf
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1., 1.],
[0., 1.]])) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q = np.eye(2)*0.0001
inf.P_inv = 0.000000000000000000001
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (50):
# create measurement = t plus white noise
z = t + random.randn()* np.sqrt(5)
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
try:
print(t, inf.P)
except:
pass
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.array([[1., 1.],
[0., 1.]]) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0, 0])
measurements.append(z)
# test mahalanobis
a = np.zeros(f.y.shape)
maha = scipy_mahalanobis(a, f.y, f.SI)
assert f.mahalanobis == approx(maha)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2., 0]]).T
f.P = np.eye(2) * 100.
s = Saver(f)
m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
s.to_array()
assert len(s.x) == len(zs)
assert len(s.x) == len(s)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, 'r', alpha=0.5)
p2, = plt.plot(results, 'b')
p4, = plt.plot(m[:, 0], 'm')
p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"], loc=4)
plt.show()
def initialize_filter(self, x_inicial):
filtro = KalmanFilter(dim_x=2, dim_z=1)
filtro.x = np.array([[x_inicial],[0.]])
filtro.F = np.array([[1.,1.],[0.,1.]])
filtro.H = np.array([[1.,0.]])
filtro.P = np.array([[10000.,0.],[0.,100.]])
filtro.R = 5.**2
filtro.Q = np.array([[0.0001,0.],[0.,0.00001]])
return filtro
def test_noisy_1d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.array([[1., 1.],
[0., 1.]]) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0, 0])
measurements.append(z)
# test mahalanobis
a = np.zeros(f.y.shape)
maha = scipy_mahalanobis(a, f.y, f.SI)
assert f.mahalanobis == approx(maha)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2., 0]]).T
f.P = np.eye(2) * 100.
s = Saver(f)
m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s)
s.to_array()
assert len(s.x) == len(zs)
assert len(s.x) == len(s)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, 'r', alpha=0.5)
p2, = plt.plot(results, 'b')
p4, = plt.plot(m[:, 0], 'm')
p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result
plt.legend([p1, p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"], loc=4)
plt.show()
def init_kalman_filter(initValue):
my_filter = KalmanFilter(dim_x=2, dim_z=1)
# initial state (location and velocity)
my_filter.x = np.array([[initValue], [0.]])
my_filter.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix
my_filter.H = np.array([[1., 0.]]) # Measurement function
my_filter.P = np.array([[1, 0.], [0., 1]]) # covariance matrix
my_filter.R = np.array([[1000.]]) # Measurement noise
my_filter.Q = np.array([[1, 1], [1, 1]]) # process noise
return my_filter
def makeLinearKF(A, B, C, P, F, x, R = None, dim_x = 4, dim_z = 4):
kf = KalmanFilter(dim_x=dim_x, dim_z=dim_z)
kf.x = x
kf.P = P
kf.F = F
kf.H = C
kf.R = R
return kf
def test_1d_0P():
global inf
f = KalmanFilter(dim_x=2, dim_z=1)
inf = InformationFilter(dim_x=2, dim_z=1)
f.x = np.array([[2.], [0.]]) # initial state (location and velocity)
f.F = (np.array([[1., 1.], [0., 1.]])) # state transition matrix
f.H = np.array([[1., 0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2) * 0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1., 0.]]) # Measurement function
inf.R_inv *= 1. / 5 # state uncertainty
inf.Q = np.eye(2) * 0.0001
inf.P_inv = 0.000000000000000000001
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range(50):
# create measurement = t plus white noise
z = t + random.randn() * np.sqrt(5)
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
try:
print(t, inf.P)
except:
pass
# save data
r.append(f.x[0, 0])
r2.append(inf.x[0, 0])
m.append(z)
#assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}'
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def kalman_initialise(self, dim_x=3, dim_z=2, dt=0.5):
kfilter = KalmanFilter(dim_x, dim_z)
kfilter.x = np.array([88., 40., 0.])
kfilter.F = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]],
dtype=float)
kfilter.H = np.array([[1, 0, 0], [0, 1, 0]], dtype=float)
kfilter.P = Q_discrete_white_noise(dim_x, 1, 1)
kfilter.Q = 0.01 * np.array([[0.1, 0, 0], [0, 10, 0], [0, 0, 100]],
dtype=float)
kfilter.R = np.array([[1, 0], [0, 1]], dtype=float)
return kfilter
def run_tracker():
# parameters
filter_misestimation_factor = 1.0
sample_size = 100
used_taps = int(sample_size * 0.5)
measurement_std = 3.5
# set up sensor simulator
dt = 0.1
measurement_std_list = np.asarray([measurement_std] * sample_size)
sim = SensorSim(0, 0.1, measurement_std_list, 1, timestep=dt)
# set up kalman filter
tracker = KalmanFilter(dim_x=2, dim_z=1)
tracker.F = np.array([[1, dt], [0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01)
tracker.Q = q
tracker.H = np.array([[1, 0]])
tracker.R = measurement_std**2 * filter_misestimation_factor
tracker.x = np.array([[0, 0]]).T
tracker.P = np.eye(2) * 500
# perform sensor simulation and filtering
readings = []
truths = []
mu = []
residuals = []
for _ in measurement_std_list:
reading, truth = sim.read()
readings.extend(reading.flatten())
truths.extend(truth.flatten())
tracker.predict()
tracker.update(reading)
mu.extend(tracker.x[0])
residual_posterior = reading - np.dot(tracker.H, tracker.x)
residuals.extend(residual_posterior[0])
# error = np.asarray(truths) - mu
# plot_results(readings, mu, error, residuals)
# perform estimation
cor = Correlator(residuals)
correlation = cor.autocorrelation(used_taps)
R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P,
'posterior')
abs_err = measurement_std**2 - R_approx
rel_err = abs_err / measurement_std**2
print("True: %.3f" % measurement_std**2)
print("Filter: %.3f" % tracker.R)
print("Estimated (approximation): %.3f" % R_approx)
print("Absolute error: %.3f" % abs_err)
print("Relative error: %.3f %%" % (rel_err * 100))
print("-" * 15)
return rel_err
def create_linear_kalman_filter_1d(dt, discrete_white_noise_var, r, x, p):
f = np.array([[1, dt, 0.5 * dt * dt], [0, 1, dt], [0, 0, 1]])
q = Q_discrete_white_noise(dim=3, dt=dt, var=discrete_white_noise_var)
tracker = KalmanFilter(dim_x=3, dim_z=1)
tracker.F = f
tracker.Q = q
tracker.x = np.array([x]).T
tracker.P = p
tracker.R = np.eye(1) * r
tracker.H = np.array([[1.0, 0.0, 0.0]])
return tracker
def rot_box_kalman_filter(initial_state, Q_std, R_std):
"""
Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
position, centroid velocity, dimensions, and rotation angle.
Parameters
----------
initial_state : sequence of floats
[x, vx, y, vy, w, h, phi]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=7, dim_z=5)
dt = 1.0 # time step
# state mean and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.eye(kf.dim_x)
kf.F[0, 1] = kf.F[2, 3] = dt
# measurement matrix - maps from state space to observation space, so
# shape is dim_z x dim_x.
kf.H = np.zeros([kf.dim_z, kf.dim_x])
# z = Hx. H has nonzero coefficients for the following components of kf.x:
# x y w h phi
kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance for x-vx or y-vy pairs
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
# diagonal process noise sub-matrix for width, height, and phi
qq = Q_std**2 * np.eye(3)
# process noise covariance matrix for full state
kf.Q = block_diag(q, q, qq)
return kf
def rot_box_kalman_filter(initial_state, Q_std, R_std):
"""
Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
position, centroid velocity, dimensions, and rotation angle.
Parameters
----------
initial_state : sequence of floats
[x, vx, y, vy, w, h, phi]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=7, dim_z=5)
dt = 1.0 # time step
# state mean and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.eye(kf.dim_x)
kf.F[0, 1] = kf.F[2, 3] = dt
# measurement matrix - maps from state space to observation space, so
# shape is dim_z x dim_x.
kf.H = np.zeros([kf.dim_z, kf.dim_x])
# z = Hx. H has nonzero coefficients for the following components of kf.x:
# x y w h phi
kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = kf.H[4, 6] = 1.0
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance for x-vx or y-vy pairs
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
# diagonal process noise sub-matrix for width, height, and phi
qq = Q_std**2*np.eye(3)
# process noise covariance matrix for full state
kf.Q = block_diag(q, q, qq)
return kf
def test_1d_0P():
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1.,1.],
[0.,1.]])) # state transition matrix
f.H = np.array([[1.,0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q *= 0.0001
inf.P_inv = 0
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def test_1d_0P():
f = KalmanFilter (dim_x=2, dim_z=1)
inf = InformationFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = (np.array([[1.,1.],
[0.,1.]])) # state transition matrix
f.H = np.array([[1.,0.]]) # Measurement function
f.R = np.array([[5.]]) # state uncertainty
f.Q = np.eye(2)*0.0001 # process uncertainty
f.P = np.diag([20., 20.])
inf.x = f.x.copy()
inf.F = f.F.copy()
inf.H = np.array([[1.,0.]]) # Measurement function
inf.R_inv *= 1./5 # state uncertainty
inf.Q *= 0.0001
inf.P_inv = 0
#inf.P_inv = inv(f.P)
m = []
r = []
r2 = []
zs = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.predict()
f.update(z)
inf.predict()
inf.update(z)
# save data
r.append (f.x[0,0])
r2.append (inf.x[0,0])
m.append(z)
#assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12
if DO_PLOT:
plt.plot(m)
plt.plot(r)
plt.plot(r2)
def newTracker(self, dt, var=0.06):
tracker = KalmanFilter(dim_x=4, dim_z=2)
tracker.x = np.array([0., 0., 0., 0.])
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
tracker.P = np.eye(4) * 500.
tracker.R = np.array([[5., 0.], [0., 5.]])
tracker.u = 0.
Q = Q_discrete_white_noise(dim=2, dt=dt, var=var)
Q = block_diag(Q, Q)
tracker.Q = Q
return tracker
def tracker1():
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
tracker.u = 0
tracker.H = np.array([[1 / 0.3048, 0, 0, 0], [0, 0, 1 / 0.3048, 0]])
tracker.R = np.eye(2) * R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500
return tracker
def tracker1(x_initial, R_std, Q_std):
tracker = KalmanFilter(dim_x=1, dim_z=1)
tracker.F = np.array([1])
tracker.u = 0.
tracker.H = np.array([1])
tracker.R = R_std
tracker.Q = Q_std
tracker.x = np.array([x_initial]).T
tracker.P = 50
return tracker
def FirstOrderKF(R, Q, dt):
""" Create first order Kalman filter.
Specify R and Q as floats."""
kf = KalmanFilter(dim_x=4, dim_z=4)
kf.x = np.array([[0, 0, 0, 0]]).T
kf.P = np.eye(4) * 0.0001
kf.R = [[0.04017680, 0., 0., 0.], [0., 0.04082702, 0., 0.],
[0., 0., 0.0002017680, 0.], [0., 0., 0., 0.0002017680]]
#np.eye(3) * R
kf.Q = np.eye(4) * Q
kf.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
kf.H = np.array([[0, 1., 0, 0], [0, 0, 0, 1.], [1., 0, 0, 0],
[0, 0, 1., 0]])
return kf
def init_tracker():
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0 # time step
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
tracker.R = np.eye(2) * 1000
q = Q_discrete_white_noise(dim=2, dt=dt, var=1)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[2000, 0, 700, 0]]).T
tracker.P = np.eye(4) * 50.0
return tracker
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1.,0.]])
P = np.eye(2)
R = np.eye(1)*std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range (2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append (x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def box_kalman_filter(initial_state, Q_std, R_std):
"""
Tracks a 2D rectangular object (e.g. a bounding box) whose state includes
position, velocity, and dimensions.
Parameters
----------
initial_state : sequence of floats
[x, vx, y, vy, w, h]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=6, dim_z=4)
dt = 1.0 # time step
# state mean and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.eye(kf.dim_x)
kf.F[0, 1] = kf.F[2, 3] = dt
# measurement matrix - maps from state space to observation space, so
# shape is dim_z x dim_x. Set coefficients for x,y,w,h to 1.0.
kf.H = np.zeros([kf.dim_z, kf.dim_x])
kf.H[0, 0] = kf.H[1, 2] = kf.H[2, 4] = kf.H[3, 5] = 1.0
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance for x-vx or y-vy pairs
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
# assume width and height are uncorrelated
q_wh = np.diag([Q_std**2, Q_std**2])
kf.Q = block_diag(q, q, q_wh)
return kf
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1) * std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append(x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def setup():
F = generate_F_matrix(velocity=0.001)
H = np.array([[0, 1]])
sim = PlaybackSensor("data/vehicle_state.json",
["fVx", "fYawrate", "fStwAng"])
# set up kalman filter
tracker = KalmanFilter(dim_x=2, dim_z=1)
tracker.F = F
tracker.Q = np.eye(2) * 0.001
tracker.H = H
tracker.R = measurement_var
tracker.x = np.array([[0, 0]]).T
tracker.P = np.eye(2) * 500
return sim, tracker
def tracker1(self):
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0 # time step
tracker.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt],
[0, 0, 0, 1]])
tracker.u = 0.
tracker.H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
tracker.R = np.eye(2) * self.R_std**2
q = Q_discrete_white_noise(dim=2, dt=dt, var=self.Q_std**2)
tracker.Q = block_diag(q, q)
tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.
return tracker
def createPersonKF(x, y):
kalman_filter = KalmanFilter(dim_x=6, dim_z=2)
# kalman_filter = KalmanFilter(dim_x=4, dim_z=4)
dt = 0.1
dt2 = 0.005
# KF_F = np.array([[1., dt, 0 , 0],
# [0 , 1., 0 , 0],
# [0 , 0 , 1., dt],
# [0 , 0 , 0 , 1.]])
KF_Fca = np.array([[1.0, dt, dt2 / 2], [0, 1.0, dt], [0, 0, 1]])
KF_F = np.vstack(
(np.hstack((KF_Fca, np.zeros((3, 3)))), np.hstack((np.zeros((3, 3)), KF_Fca))) # np.zeros((3,3)))),
) # , np.zeros((3,3)))) )))#,
# np.hstack((np.zeros((3,3)),np.zeros((3,3)), KF_Fca))))
KF_q = 0.7 # 0.3
# KF_Q = np.vstack((np.hstack((Q_discrete_white_noise(2, dt=0.1, var=KF_q),np.zeros((2,2)))),np.hstack((np.zeros((2,2)),Q_discrete_white_noise(2, dt=0.1, var=KF_q)))))
KF_Q = np.vstack(
(
np.hstack((Q_discrete_white_noise(3, dt=0.1, var=KF_q), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), Q_discrete_white_noise(3, dt=0.1, var=KF_q))),
)
)
KF_pd = 25.0
KF_pv = 10.0
KF_pa = 30.0
KF_P = np.diag([KF_pd, KF_pv, KF_pa, KF_pd, KF_pv, KF_pa])
KF_rd = 0.05
KF_rv = 0.2 # 0.5
KF_ra = 2 # 0.5
KF_R = np.diag([KF_rd, KF_rd])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv])
# KF_R = np.diag([KF_rd,KF_rd, KF_rv, KF_rv, KF_ra, KF_ra])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0]])
# KF_H = np.array([[1.,0,0,0],[0,0,1.,0],[0,1.,0,0],[0,0,0,1.]])
KF_H = np.array([[1.0, 0, 0, 0, 0, 0], [0, 0, 0, 1.0, 0, 0]])
# kalman_filter.x = np.array([x,0,y,0])
kalman_filter.x = np.array([x, 0, 0, y, 0, 0])
kalman_filter.F = KF_F
kalman_filter.H = KF_H
kalman_filter.Q = KF_Q
kalman_filter.B = 0
kalman_filter.R = KF_R
kalman_filter.P = KF_P
return kalman_filter
def test_noisy_11d():
f = KalmanFilter(dim_x=2, dim_z=1)
f.x = np.array([2.0, 0]) # initial state (location and velocity)
f.F = np.array([[1.0, 1.0], [0.0, 1.0]]) # state transition matrix
f.H = np.array([[1.0, 0.0]]) # Measurement function
f.P *= 1000.0 # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range(100):
# create measurement = t plus white noise
z = t + random.randn() * 20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
# save data
results.append(f.x[0])
measurements.append(z)
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2.0, 0]]).T
f.P = np.eye(2) * 100.0
m, c, _, _ = f.batch_filter(zs, update_first=False)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements, "r", alpha=0.5)
p2, = plt.plot(results, "b")
p4, = plt.plot(m[:, 0], "m")
p3, = plt.plot([0, 100], [0, 100], "g") # perfect result
plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4)
plt.show()
def const_vel_filter(dt, x0=0, x_ndim=1, P_diag=(1., 1.), R_std=1., Q_var=0.0001):
""" helper, constructs 1d, constant velocity filter"""
f = KalmanFilter(dim_x=2, dim_z=1)
if x_ndim == 1:
f.x = np.array([x0, 0.])
else:
f.x = np.array([[x0, 0.]]).T
f.F = np.array([[1., dt],
[0., 1.]])
f.H = np.array([[1., 0.]])
f.P = np.diag(P_diag)
f.R = np.eye(1) * (R_std**2)
f.Q = Q_discrete_white_noise(2, dt, Q_var)
return f
def point_2d_kalman_filter(initial_state, Q_std, R_std):
"""
Parameters
----------
initial_state : sequence of floats
[x0, vx0, y0, vy0]
Q_std : float
Standard deviation to use for process noise covariance matrix
R_std : float
Standard deviation to use for measurement noise covariance matrix
Returns
-------
kf : filterpy.kalman.KalmanFilter instance
"""
kf = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.0 # time step
# state mean (x, vx, y, vy) and covariance
kf.x = np.array([initial_state]).T
kf.P = np.eye(kf.dim_x) * 500.
# no control inputs
kf.u = 0.
# state transition matrix
kf.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
# measurement matrix - maps from state space to observation space
kf.H = np.array([[1, 0, 0, 0],
[0, 0, 1, 0]])
# measurement noise covariance
kf.R = np.eye(kf.dim_z) * R_std**2
# process noise covariance
q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
kf.Q = block_diag(q, q)
return kf
def init_filter(dt):
states = 18 # number of states, first and second order derivatives
obs = 6 # observations
filter = KalmanFilter(dim_x=states,dim_z=obs)
filter.x = np.zeros([1,states]).T
transition_mat = np.eye(9)
for i in range(0,9):
id_vel = i + 3
id_acc = i + 6
if id_vel < 9:
transition_mat[i,id_vel] = dt
if id_acc < 9:
transition_mat[i,id_acc] = 0.5 * dt * dt
zero_mat = np.zeros([9,9])
tmp1 = np.hstack([transition_mat,zero_mat])
tmp2 = np.hstack([zero_mat,transition_mat])
filter.F = np.vstack([tmp1,tmp2])
filter.H = np.zeros([obs,states])
filter.H[0,0] = 1
filter.H[1,1] = 1
filter.H[2,2] = 1
filter.H[3,9] = 1
filter.H[4,10] = 1
filter.H[5,11] = 1
filter.Q = np.eye(states) * 1e-4; # process noise
filter.R = np.eye(obs) * 0.01 # measurement noise
filter.P = np.eye(states) * 1e-4 # covariance post
filter.u = 0.
return filter
def init_kalman():
filter = KalmanFilter(dim_x=4,dim_z=2)
dt = 0.01
filter.x = np.array([[0,0,0,0]]).T
filter.F = np.array([[1,0,dt,0],
[0,1,0,dt],
[0,0,1,0],
[0,0,0,1]])
#q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
#filter.Q = block_diag(q, q)
filter.Q = np.eye(4) * 1e-4; # process noise
print(filter.Q)
filter.H = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
filter.R = np.array([[0.1, 0],[0, 0.1]]) # measurement noise
filter.P = np.eye(4) * 1e-4 # covariance post
filter.u = 0.
return filter
def test_procedural_batch_filter():
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.array([2., 0])
f.F = np.array([[1.,1.],
[0.,1.]])
f.H = np.array([[1.,0.]])
f.P = np.eye(2)*1000.
f.R = np.eye(1)*5
f.Q = Q_discrete_white_noise(2, 1., 0.0001)
f.test_matrix_dimensions()
x = np.array([2., 0])
F = np.array([[1.,1.],
[0.,1.]])
H = np.array([[1., 0.]])
P = np.eye(2)*1000.
R = np.eye(1)*5
Q = Q_discrete_white_noise(2, 1., 0.0001)
zs = [13., None, 1., 2.]*10
m,c,_,_ = f.batch_filter(zs,update_first=False)
n = len(zs)
# test both list of matrices, and single matrix forms
mp, cp, _, _ = batch_filter(x, P, zs, F, Q, [H]*n, R)
for x1, x2 in zip(m, mp):
assert np.allclose(x1, x2)
for p1, p2 in zip(c, cp):
assert np.allclose(p1, p2)
import cv2
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
####################################
x_kalman = KalmanFilter(dim_x=2,dim_z=1)
x_kalman.x = np.array([[0.], # position
[0.]]) # velocity
x_kalman.F = np.array([[1.,1.],
[0.,1.]])
x_kalman.H = np.array([[1.,0.]])
x_kalman.P = x_kalman.P*1000.0 #.array([[1000., 0.],
# [ 0., 1000.] ])
x_kalman.R = np.array([[5.]])
x_kalman.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
###################################
y_kalman = KalmanFilter(dim_x=2,dim_z=1)
y_kalman.x = np.array([[0.], # position
[0.]]) # velocity
y_kalman.F = np.array([[1.,1.],
[0.,1.]])
# See Chapter 08
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = times[1] - times[0]
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
tracker.Q = block_diag(q, q)
tracker.B = 0
tracker.H = np.array([[1.0, 0, 0, 0],
[0 , 0, 1, 0]])
tracker.R = np.array([[0.1, 0],
[0, 0.1]])
tracker.x = np.array([[x, y, 0, 0]]).T
tracker.P = np.eye(4)*10.
# Convert measurements to expected form
# Note: n x 4 x 1 np array, measurements must be a col vector
measurements = np.array([np.array([pos]).T for pos in positions])
mu, cov, _, _ = tracker.batch_filter(measurements)
f3 = plt.figure()
plt.scatter(xs, ys)
plt.plot(mu[:,0], mu[:,2])
plt.title('Comparison of Measurements and Kalman Filter')
plt.xlabel('x')
plt.ylabel('y')
f3.show()
def getPos(self):
return [self.pos[0], self.pos[1]]
tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.
tracker.F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
tracker.u = 0.
tracker.H = np.array([[1/0.3084, 0, 0, 0],
[0, 0, 1/0.3084, 0]])
tracker.R = np.array([[5,0],
[0,5]])
tracker.Q = np.eye(4) * 0.1
tracker.P = np.eye(4) * 500
count = 80
tx, ty = [],[]
xs, ys = [],[]
pxs, pys = [],[]
sensor = PosSensor1([0,0], (2,2), 1)
for i in range(count):
tp = sensor.getPos()
tx.append(tp[0]*.3084)
ty.append(tp[1]*.3084)
pos = sensor.read()
z = np.array([[pos[0]], [pos[1]]])
tracker.predict()
tracker.update(z)
if __name__ == '__main__':
rospy.init_node('particle_filter', anonymous=True)
rospy.Subscriber("/poi", PointStamped, observation_callback)
rospy.Subscriber("/ardrone/bottom/image_raw", Image, img_callback, queue_size=5)
error_pub = rospy.Publisher("/error", PointStamped, queue_size=5)
frame = None
observation = None
f = KalmanFilter(dim_x=2, dim_z=2)
f.x = np.array([[0.], [0.]]) # initial state (location and velocity)
f.F = np.array([[1.0002,0.],
[0.,1.0002]]) # state transition matrix
f.P = np.array([[10.0, 0.0],
[0.0, 10.0]])
f.R = np.array([[0.0001, 0.0],
[0.0, 0.0001]])
f.H = np.array([[1.0, 0.0],
[0.0, 1.0]])
f.Q = Q_discrete_white_noise(dim = 2, dt = 100.0, var = 5000.0)
rate = rospy.Rate(100) # 10hz
cv2.namedWindow("Kalman Filter Output",1)
while not rospy.is_shutdown():
print "Prior Belief :", f.x
f.predict()
x_bar, covar = f.get_prediction()
from numpy import dot, zeros, eye, isscalar
def CO2_kf_filter(CO2, param):
"""filter matrices initialization for KF"""
try:
theta = param.theta
Qparam = param.Qparam
except AttributeError, e:
raise e
x_dim = CO2.x_dim
z_dim = CO2.H_mtx.shape[0]
# theta = (1.14,1e-5)
kf = KalmanFilter(x_dim, z_dim)
kf.P = np.zeros((x_dim, x_dim))
kf.x = np.zeros((x_dim, 1))
kf.H = CO2.H_mtx
kf.F = np.identity(x_dim)
kf.R = theta[1] * kf.R
grid = CO2.grid
# create Q
Q = common.getGCF(grid, Qparam[0], Qparam[1])
kf.Q = theta[0] * theta[1] * Q
return kf
def CO2_hikf_filter(CO2, param):
"""HiKF filter matrices initialization"""
try:
theta = param.theta
def test_noisy_1d():
f = KalmanFilter (dim_x=2, dim_z=1)
f.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
f.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
f.H = np.array([[1.,0.]]) # Measurement function
f.P *= 1000. # covariance matrix
f.R = 5 # state uncertainty
f.Q = 0.0001 # process uncertainty
fsq = SquareRootKalmanFilter (dim_x=2, dim_z=1)
fsq.x = np.array([[2.],
[0.]]) # initial state (location and velocity)
fsq.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
fsq.H = np.array([[1.,0.]]) # Measurement function
fsq.P = np.eye(2) * 1000. # covariance matrix
fsq.R = 5 # state uncertainty
fsq.Q = 0.0001 # process uncertainty
measurements = []
results = []
zs = []
for t in range (100):
# create measurement = t plus white noise
z = t + random.randn()*20
zs.append(z)
# perform kalman filtering
f.update(z)
f.predict()
fsq.update(z)
fsq.predict()
assert abs(f.x[0,0] - fsq.x[0,0]) < 1.e-12
assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-12
# save data
results.append (f.x[0,0])
measurements.append(z)
p = f.P - fsq.P
print(f.P)
print(fsq.P)
for i in range(f.P.shape[0]):
assert abs(f.P[i,i] - fsq.P[i,i]) < 0.01
# now do a batch run with the stored z values so we can test that
# it is working the same as the recursive implementation.
# give slightly different P so result is slightly different
f.x = np.array([[2.,0]]).T
f.P = np.eye(2)*100.
m,c,_,_ = f.batch_filter(zs,update_first=False)
# plot data
if DO_PLOT:
p1, = plt.plot(measurements,'r', alpha=0.5)
p2, = plt.plot (results,'b')
p4, = plt.plot(m[:,0], 'm')
p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
plt.legend([p1,p2, p3, p4],
["noisy measurement", "KF output", "ideal", "batch"], 4)
plt.show()
from numpy import random
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
if __name__ == '__main__':
fk = KalmanFilter(dim_x=2, dim_z=1)
fk.x = np.array([-1., 1.]) # initial state (location and velocity)
fk.F = np.array([[1.,1.],
[0.,1.]]) # state transition matrix
fk.H = np.array([[1.,0.]]) # Measurement function
fk.P = .01 # covariance matrix
fk.R = 5 # state uncertainty
fk.Q = 0.001 # process uncertainty
zs = [t + random.randn()*4 for t in range (40)]
mu, cov, _, _ = fk.batch_filter (zs)
mus = [x[0] for x in mu]
M,P,C = fk.rts_smoother(mu, cov)
# plot data
p1, = plt.plot(zs,'cyan', alpha=0.5)
[0, 0, 1, 0]]) # Measurement function f.H = np.array([[1., 1., 0., 0.]]) # Measurement noise f.R = np.array([[sigma_epsilon **2]]) # Measurement noise covariance f.Q = np.diag([sigma_xi ** 2, sigma_omega ** 2, 0.0, 0.0]) # Run the recursive flow # initial state, period 1 f.x = np.array([18.0, 5, -12, -8]) # initial covariance has a nice identity default f.P = np.diag([1, 1, 1, 1]) * 1. # Here's how to match the simdkalman, which updates the initial state first! # Echo initial states f.x f.P f.K f.update(series[0]) # has to be a measurement from the right period! f.x # a posteriori estimate of x f.P # a posteriori estimate of P f.K # Now there is a K # Go to the next time point f.predict()
# -*- coding: utf-8 -*- """ Created on Thu Jun 18 09:16:54 2015 @author: rlabbe """ from filterpy.kalman import KalmanFilter kf = KalmanFilter(1, 1) kf.x[0,0] = 1. kf.P = np.ones((1,1)) kf.H = np.array([[2.]]) kf.F = np.ones((1,1)) kf.R = 1 kf.Q = 0 kf.predict() kf.update(2)
