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_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 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 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 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 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 setup_KF(x, y, z=None):
flag_2d = z is None
if flag_2d:
kf = KalmanFilter(dim_x=4, dim_z=2)
kf.F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0],
[0, 0, 0, 1]])
kf.H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
])
kf.x[0] = x
kf.x[1] = y
else:
kf = KalmanFilter(dim_x=6, dim_z=3)
kf.F = np.array([[1, 0, 0, 1, 0, 0], [0, 1, 0, 0, 1, 0],
[0, 0, 1, 0, 0, 1], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
kf.H = np.array([
[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
])
kf.x[0] = x
kf.x[1] = y
kf.x[2] = z
# kf.R[2:, 2:] *= 10.
# kf.P[4:, 4:] *= 1000. # give high uncertainty to the unobservable initial velocities
# kf.P *= 10.
# kf.Q[-1, -1] *= 0.01
# kf.Q[4:, 4:] *= 0.01
kf.B = np.eye(kf.dim_u)
kf.get_pred = lambda: np.dot(kf.F, kf.x)
return kf
def main():
[t, dt, s, v, a, theta, omega, alpha] = build_real_values()
zs = build_measurement_values(t, [a, omega])
u = build_control_values(t, v)
[F, B, H, Q, R] = init_kalman(t, dt)
kf = KalmanFilter(9, 3, 2)
kf.x = np.array([[0., 0., 0., 0., 0., 0., 0., 0., 0.]]).T
kf.B = B
kf.R = R
kf.F = F
kf.H = H
kf.Q = Q
xs, cov = [], []
for zk, uk in zip(zs, u):
kf.predict(u=uk)
kf.update(z=zk)
xs.append(kf.x)
cov.append(kf.P)
xs, cov = np.array(xs), np.array(cov)
xground = construct_xground(s, v, a, theta, omega, alpha, xs.shape)
nees = NEES(xground, xs, cov)
print(np.mean(nees))
plot_results(t, xs, xground, zs, nees)
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 getKalmanFilter(m):
kalman = KalmanFilter(len(m) * 2, len(m))
kalman.x = np.hstack((m, [0.0, 0.0])).astype(np.float)
kalman.F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0],
[0, 0, 0, 1]])
kalman.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
kalman.P *= 1000
kalman.R = 0.00001
kalman.Q = Q_discrete_white_noise(dim=4, dt=1, var=5)
kalman.B = 0
return kalman
def test_univariate():
f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
f.x = np.array([[0]])
f.P *= 50
f.H = np.array([[1.]])
f.F = np.array([[1.]])
f.B = np.array([[1.]])
f.Q = .02
f.R *= .1
for i in range(50):
f.predict()
f.update(i)
def test_univariate():
f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
f.x = np.array([[0]])
f.P *= 50
f.H = np.array([[1.]])
f.F = np.array([[1.]])
f.B = np.array([[1.]])
f.Q = .02
f.R *= .1
for i in range(50):
f.predict()
f.update(i)
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U):
'''
Function creates a Kalman Filter object and assigns all the instance variables
:return: Kalman Filter
'''
kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z)
kf.x = X
kf.P = P
kf.F = A
print('=======================')
kf.Q = Q
kf.B = B
kf.U = U
kf.R = R
kf.H = H
return kf
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 __init__(self, dim_state, dim_control, dim_measurement,
initial_state_mean, initial_state_covariance, matrix_F,
matrix_B, process_noise_Q, matrix_H, measurement_noise_R):
filter = KalmanFilter(dim_x=dim_state,
dim_u=dim_control,
dim_z=dim_measurement)
filter.x = initial_state_mean
filter.P = initial_state_covariance
filter.Q = process_noise_Q
filter.F = matrix_F
filter.B = matrix_B
filter.H = matrix_H
filter.R = measurement_noise_R # covariance matrix
super().__init__(filter)
def create_kf_and_assign_predict_update(dim_z, X, P, A, Q, dt, R, H, B, U):
'''
:param configs tuple: all the values to define the kalman filter
:return: Kalman Filter
'''
kf = KalmanFilter(dim_x=X.shape[0], dim_z=dim_z)
kf.x = X
kf.P = P
kf.F = A
print('=======================')
kf.Q = Q
kf.B = B
kf.U = U
kf.R = R
kf.H = H
return kf
def make_filter(start):
f = KalmanFilter(dim_x=6, dim_z=3)
f.x = start
f.F = np.eye(6)
f.F[:3, 3:6] = np.eye(3) * dt
f.B = np.array([0, dt**2 / 2, 0, 0, dt, 0])
g = -9.81
f.H = np.zeros((3, 6))
f.H[:, :3] = np.eye(3)
f.Q *= 0.5
f.R *= 2
f.P *= 1
return f
def tof_filter_init(self):
'''ToF Filter'''
dt = 0.01 # Just random assumation
tof_filter = KalmanFilter(dim_x=2, dim_z=2,
dim_u=1) # Set up the ToF filter
tof_filter.F = np.array([
[1, dt], # The Sensor Model
[0, 1]
])
tof_filter.P = np.diag([0.1, 0.1]) # covariance matrix
tof_filter.B = np.array([
[0], # Control Matrix
[dt]
])
tof_filter.H = np.diag([1., 1.]) # Measurement Matrix
tof_filter.Q = np.diag([0.9, 0.4]) # Process covariance
tof_filter.R = np.diag([
0.02**2, 0.05**2
]) # Measurement covariance # Noise of he sensor ~0.01m (1cm)
return tof_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 createTracker(iniVector):
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, 0, 0],
[0, 0, 0, 0]])
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.])
q = Q_discrete_white_noise(dim=2, dt=dt, var=0.04**2)
tracker.Q = block_diag(q, q)
tracker.x = iniVector
tracker.P = np.diag([3,3,3,3])
return tracker
return self.attack_sequence, Gamma
if __name__ == "__main__":
# ================== Filter and system generation ========================
kf = KalmanFilter(dim_x=2,dim_z=2)
kf.x = [0.,0.] # Initial values state-space is [ x xdot ]'
kf.H = np.array([[1.,0.], # Observation matrix
[0.,1.]])
kf.F = np.array([[1., 1.],
[0., 1.]]) # State transition matrix
kf.R = np.eye(2)
kf.Q = np.eye(2)
kf.P = np.array([[1., 0.],
[0., 1.]])
kf.B = np.array([[0.5, 1.]]).T
xs = kf.x # Initial values for the data generation
zs = [[kf.x[0],kf.x[1]]] # We are measuring both the position and velocity
pos = [kf.x[0]] # Initialization of the true position values
vel = [kf.x[1]] # Initialization of the true velocity values
noise_std = 1.
# ==========================================================================
# ======== Noisy position measurements generation for 30 samples ===========
for _ in range(30):
last_pos = xs[0]
last_vel = xs[1]
new_vel = last_vel
new_pos = last_pos + last_vel
xs = [new_pos, new_vel]
z = [new_pos + (randn()*noise_std), new_vel + (randn()*noise_std)]
plt.xlabel('x')
plt.ylabel('y')
f2.show()
# Basic Kalman filter
# https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
# 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)
def kalclip(bbox, gt, model, clippath, stats, ofpoint=0, R=500, Q=500, P=0):
#define start(no need to initiate before detections)
start = h.getstartindex(bbox)
clipname = h.extract(clippath)
#for 30fps --> 1/fps
#irrelevant in my case --> set 1 (i dont rly make a reference to reality here)
deltat = 1
#define what model has what representation for retransformation after:
correp = []
cenrep = ["cenof", "simplecen"]
asprep = ["aspof", "aspofwidth", "aspofwidthman"]
std = 70
#define dimensions and model(depends on representation)
if model == "simplecen":
#transform representation
bbox = h.corcen(bbox)
gt = h.corcen(gt)
#set parameters
n_x, n_z = 4, 4
Kal = KalmanFilter(dim_x=n_x, dim_z=n_z)
Kal.F = np.eye(n_x)
Kal.H = np.zeros((n_z, n_x))
Kal.H[0][0] = Kal.H[1][1] = Kal.H[2][2] = Kal.H[3][3] = 1
Kal.Q = np.eye(n_x) * std**2
for i in range(len(stats)):
Kal.R[i, i] = stats[0, 1, i]**2
#init
init = np.zeros(n_x)
init[:] = gt[0, :]
memp = np.zeros((bbox.shape[0], n_x))
memk = np.zeros((bbox.shape[0], n_x))
elif model == "cenof":
#transform representation
bbox = h.corcen(bbox)
gt = h.corcen(gt)
#set parameters
n_x, n_z, n_u = 4, 4, 2
Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u)
Kal.R = np.eye(n_z) * R
for i in range(len(stats)):
Kal.R[i, i] = stats[1, 1, i]**2
Kal.Q = np.eye(n_x) * std**2
Kal.Q[0, 0], Kal.Q[1, 1] = stats[3, 1, 0], stats[3, 1, 1]
Kal.P *= P
Kal.F = np.eye(n_x)
Kal.H = np.zeros((n_z, n_x))
Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1
Kal.B = np.eye(n_x, n_u)
#init
init = np.zeros(n_x)
init[:4] = gt[0, :]
memp = np.zeros((bbox.shape[0], n_x))
memk = np.zeros((bbox.shape[0], n_x))
elif model == "aspof":
#transform representation
bbox = h.corasp(bbox)
gt = h.corasp(gt)
#set parameters
n_x, n_z, n_u = 4, 4, 2
Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u)
Kal.R = np.eye(n_z) * R
for i in range(len(stats)):
Kal.R[i, i] = stats[2, 1, i]**2
Kal.Q = np.eye(n_x) * std**2
Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[3,
3] = stats[3, 1,
0]**2, stats[3, 1,
1]**2, 0.0001
Kal.P *= P
Kal.F = np.eye(n_x)
Kal.B = np.eye(n_x, n_u)
Kal.H = np.zeros((n_z, n_x))
Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1
#init
init = np.zeros(n_x)
init[:4] = gt[0, :]
memp = np.zeros((bbox.shape[0], n_x))
memk = np.zeros((bbox.shape[0], n_x))
elif model == "aspofwidth":
#transform representation
bbox = h.corasp(bbox)
gt = h.corasp(gt)
#set parameters
n_x, n_z, n_u = 4, 4, 3
Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u)
Kal.R = np.eye(n_z) * R
for i in range(len(stats)):
Kal.R[i, i] = stats[2, 1, i]**2
Kal.Q = np.eye(n_x) * std**2
#TODO put in stats for std width from of
Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[2, 2], Kal.Q[3, 3] = stats[
3, 1, 0]**2, stats[3, 1, 1]**2, stats[3, 1, 2]**2, 0.0001
Kal.P *= P
Kal.F = np.eye(n_x)
Kal.B = np.eye(n_x, n_u)
Kal.H = np.zeros((n_z, n_x))
Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1
#init
init = np.zeros(n_x)
init[:4] = gt[0, :]
memp = np.zeros((bbox.shape[0], n_x))
memk = np.zeros((bbox.shape[0], n_x))
elif model == "aspofwidthman":
#transform representation
bbox = h.corasp(bbox)
gt = h.corasp(gt)
#set parameters
n_x, n_z, n_u = 4, 4, 3
Kal = KalmanFilter(dim_x=n_x, dim_z=n_z, dim_u=n_u)
Kal.R = np.eye(n_z) * R
for i in range(len(stats)):
Kal.R[i, i] = stats[2, 1, i]**2
Kal.Q = np.eye(n_x) * std**2
Kal.Q[0, 0], Kal.Q[1, 1], Kal.Q[2, 2], Kal.Q[3, 3] = stats[
3, 1, 0], stats[3, 1, 1], 1**2, 0.0001
Kal.P *= P
Kal.F = np.eye(n_x)
Kal.B = np.eye(n_x, n_u)
Kal.H = np.zeros((n_z, n_x))
Kal.H[0, 0] = Kal.H[1, 1] = Kal.H[2, 2] = Kal.H[3, 3] = 1
#init
init = np.zeros(n_x)
init[:4] = gt[0, :]
memp = np.zeros((bbox.shape[0], n_x))
memk = np.zeros((bbox.shape[0], n_x))
#parameters to define(try these on all representations)
Kal.x = init
#create new array for results of the algorithm
mem = ma.array(np.zeros_like(bbox), mask=True)
mem[0, :] = init
for j in range(memp.shape[1]):
memp[0, j] = Kal.P[j, j]
for j in range(memk.shape[1]):
memk[0, j] = Kal.K[j, j]
for i in range(1, bbox.shape[0], 1):
z = bbox[i, :]
if bbox.mask[i, 0] == True:
z = None
if model == "cenof":
vx = ofpoint[i, 0] - ofpoint[i - 1, 0]
vy = ofpoint[i, 1] - ofpoint[i - 1, 1]
u = np.array([vx, vy])
Kal.predict(u)
elif model == "aspof":
vx = ofpoint[i, 0] - ofpoint[i - 1, 0]
vy = ofpoint[i, 1] - ofpoint[i - 1, 1]
u = np.array([vx, vy])
Kal.predict(u)
elif model == "aspofwidth":
vx = ofpoint[i, 0] - ofpoint[i - 1, 0]
vy = ofpoint[i, 1] - ofpoint[i - 1, 1]
vwidth = ofpoint[i, 2] - ofpoint[i - 1, 2]
u = np.array([vx, vy, vwidth])
Kal.predict(u)
elif model == "aspofwidthman":
vx = ofpoint[i, 0] - ofpoint[i - 1, 0]
vy = ofpoint[i, 1] - ofpoint[i - 1, 1]
vwidth = ofpoint[i, 2] - ofpoint[i - 1, 2]
u = np.array([vx, vy, vwidth])
Kal.predict(u)
else:
Kal.predict()
Kal.update(z)
x = Kal.x
y = x[:4]
#write into resulting array
mem[i, :] = y[:4]
for j in range(memp.shape[1]):
memp[i, j] = Kal.P[j, j]
for j in range(memk.shape[1]):
memk[i, j] = Kal.K[j, j]
#just return result --> do back-transformation into other representations outside of this function(as well as creating visuals)
if model in correp:
mem = mem
elif model in cenrep:
mem = h.cencor(mem)
elif model in asprep:
mem = h.aspcor(mem)
return mem, memp, memk
# Process noise (Q)
# (2, 2) = square matrix
abc_filter.Q = np.diag([s_noise, r_noise])
# ------- Update Variables -------
# Measurement function (H) (how do we go from state -> observed?)
# (1, 2) = row vector
abc_filter.H = np.array([[c, (1 - a - b)], [0, 1]])
# measurement uncertainty
# (2, 2) = square matrix OR is it just uncertainty on discharge (q)
abc_filter.R *= R
# Control inputs (defaults)
abc_filter.B = None # np.ndarray([a])
abc_filter.dim_u = 0
return abc_filter
def simulate_data(
original_data: pd.DataFrame, q_obs_noise: float, r_obs_noise: float
) -> pd.DataFrame:
assert all(np.isin(["precipitation", "discharge_spec"], original_data.columns))
data = original_data.copy()
# simulate using the ABC model
true_q, true_S = abc_simulate(data["precipitation"])
data["q_true"] = true_q
data["S_true"] = true_S
def kf(self,x_obj,y_obj):
#list posisi objek
pos_x = x_obj
pos_y = y_obj
kf_res_x = []
kf_res_y = []
kf_res_vx = []
kf_res_vy = []
def vel(alist):
vel_list = [0.]
for i in range(len(alist)):
j = i+1
# print(i)
# print(alist[i])
try:
vel_list.append(alist[j] - alist[i])
except:
pass
return vel_list
#list acceleration; input=vel_list
def accel(alist):
accel_list = [0., 0.]
try:
if not alist[2]:
pass
else:
for i in range(len(alist)):
j = i+1
k = j+1
accel_list.append(alist[k]-alist[j])
except:
pass
return accel_list
#list error
def sqrt_alist(alist): #input alist= _min_avg yang mau di kuadrat
sqrt_list=[]
for i in range(len(alist)):
sqrt_list.append(round((alist[i]**2), 2))
return sqrt_list
def avgpos(alist):
average = sum(alist) / len(alist)
average = round(average, 2)
return average
def avgvel(alist):
average = sum(alist) / (len(alist)-1)
average = round(average, 2)
return average
def pos_min_avg(alist):
min_avg = []
try:
for i in range(len(alist)):
min_aver = alist[i] - avgpos(alist)
min_avg.append(round(min_aver, 2))
except:
pass
return min_avg
def vel_min_avg(alist):
min_avg = [0.]
for i in range(len(alist)):
j = i+1
try:
min_avg.append(round((alist[j] - avgvel(alist)), 2))
except:
pass
return min_avg
#standar deviasi
def stdpos(alist): #input=list yang udh di kuadrat
n = len(alist)
std = math.sqrt( (1/(n-1)) * sum(alist) )
return std
def stdvel(alist): #input=list yang udh di kuadrat; -2 soalnya isi mulai dr index 1
n = len(alist)
std = math.sqrt( (1/(n-2)) * sum(alist) )
return std
vel_x = vel(pos_x)
vel_y = vel(pos_y)
accelx = accel(pos_x)
accely = accel(pos_y)
var_x = stdpos( sqrt_alist( pos_min_avg(pos_x) ) )
var_y = stdpos( sqrt_alist( pos_min_avg(pos_y) ) )
var_vx = stdvel( sqrt_alist( vel_min_avg(vel_x) ) )
var_vy = stdvel( sqrt_alist( vel_min_avg(vel_y) ) )
### TRACKING POSITION AND VELOCITY FROM POSITIONS ###
# CONSTRUCT OBJECTS DIMENSIONALITY (4x1 dengan 4x4)
f = KalmanFilter( dim_x = 4,
dim_z = 4 )
# ASSIGN INIT VALUES (proses mengkuti contoh soal)
f.x = np.array([ pos_x[2],
pos_y[2], #position
vel_x[2],
vel_y[2] ]) #velocity
# DEF STATE TRANSITION MATRIX
f.F = np.array([ [1., 0., 1., 0.],
[0., 1., 0., 1.],
[0., 0., 1., 0.],
[0., 0., 0., 1.] ])
f.B = np.array([ [.5, 0.],
[0., .5],
[1., 0.],
[0., 1.] ])
f.u = np.array([ accelx[2],
accely[2] ])
# DEF MEASUREMENT FUNCTION
f.H = np.array([ [1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.] ])
# DEF COVARIANCE MATRIX
f.P = np.array([ [.9*((var_x)**2), 0., 1., 0.],
[0., .9*((var_y)**2), 0., 1.],
[0., 0., .9*((var_vx)**2), 0.],
[0., 0., 0., .9*((var_vy)**2)] ])
# ASSIGN MEASUREMENT NOISE
f.R = np.array([ [(var_x)**2, 0., 1., 0.],
[0., (var_y)**2, 0., 1.],
[0., 0., (var_vx)**2, 0.],
[0., 0., 0., (var_vy)**2] ])
# ASSIGN PROCESS NOISE
f.Q = np.zeros((4,4))
#sensor input
# def sensor_read():
# a =np.array([ [231.],
# [77.], #position
# [ np.negative(26.)],
# [4.] ]) #velocity
# return a
# PREDICT UPDATE LOOP
for i in range(len(accelx)-3): #masi kebanyakan len nya mknya out of range
z = np.array([ pos_x[i+3],
pos_y[i+3], #position
vel_x[i+3],
vel_y[i+3] ]) #velocity
try:
f.u = np.array([ accelx(i+3),
accely(i+3) ])
except:
pass
f.predict()
f.update(z)
kf_res_x.append(np.round(f.x[0]))
kf_res_y.append(np.round(f.x[1]))
kf_res_vx.append(np.round(f.x[2]))
kf_res_vy.append(np.round(f.x[3]))
# print('input z:')
# print(z)
# print('===========KF============')
# print("predict: ",f.x_prior)
# print("update: ",f.x)
# print('=========================')
print('kf_x: {}'.format(kf_res_x))
print('kf_y: {}'.format(kf_res_y))
guess_noise=.9*((var_vy)**2)
# print(guess_noise)
# print(var_x,var_y,var_vx,var_vy)
# print(f.R)
out = {
'f_R': f.R.tolist(),
'kf_x': kf_res_x,
'kf_y': kf_res_y,
'kf_vx': kf_res_vx,
'kf_vy': kf_res_vy,
'guess_noise': .9
}
return out
from filterpy.kalman import KalmanFilter
import numpy as np
from filterpy.common import Q_discrete_white_noise
import matplotlib.pyplot as plt
from kf_book.book_plots import plot_measurements
from kf_book.book_plots import plot_filter
from scipy.linalg import block_diag
dt = 1.0
R = 3
kf = KalmanFilter(dim_x=2, dim_z=1, dim_u=1)
kf.P *= 10
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, 0.1)
kf.F = np.array([[1, 0], [0, 1]])
kf.B = np.array([[dt], [1]])
kf.H = np.array([[1, 0]])
zs = [i + np.random.randn() * R for i in range(1, 1000)]
xs = []
cmd_velocity = 1
for z in zs:
kf.predict(u=cmd_velocity)
kf.update(z)
kf.append(kf.x[0])
"""To save some time just run the part you want"""
run_example = False
run_real_cases = True
"""
Case from:
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/08-Designing-Kalman-Filters.ipynb
"""
if run_example:
dt = 1.
R = 3.
kf = KalmanFilter(dim_x=2, dim_z=1, dim_u=1)
kf.P *= 10
kf.R *= R
kf.Q = Q_discrete_white_noise(2, dt, 0.1)
kf.F = np.array([[1., 0], [0., 0.]])
kf.B = np.array([[dt], [ 1.]])
kf.H = np.array([[1., 0]])
print(kf.P)
zs = [i + randn()*R for i in range(1, 100)]
xs = []
cmd_velocity = 1.
for z in zs:
kf.predict(u=cmd_velocity)
kf.update(z)
xs.append(kf.x[0])
plt.clf()
plt.plot(xs, label='Kalman Filter')
plot_measurements(zs)
plt.xlabel('time')
plt.legend(loc=4)
plt.ylabel('distance')
bike.update_C(C)
sys = bike.discrete_ss(dt)
print(sys.A)
print(sys.B)
print(sys.C)
print(sys.D)
#dt = 1/60
#t_sim, x_sim = bike.continuous_response(time, np.zeros(time.size),
# x0=np.array([np.deg2rad(6), 0, 0, 0]))
#measurements = x_sim[:,2] + np.random.randn(t_sim.size)*r_var
#print(measurements)
kf = KalmanFilter(dim_x=4, dim_z=1)
kf.F = sys.A
kf.B = sys.B
kf.H = sys.C
kf.R *= r_var
kf.Q = bike.calc_process_noise(q_var, dt)
kf.P *= 1
saver = common.Saver(kf)
for idx in range(time.size):
kf.predict()
kf.update(measurements[idx])
saver.save()
saver.to_array()
plt.figure()
plt.subplot(2, 2, 1)
plt.plot(time, np.rad2deg(saver.x[:, 0, 0]))
# Generate groundtruth and measurement sequence
for i in range(1, N):
x_pre = xt_gt[..., i - 1].reshape(4, 1)
x = (F @ x_pre + B * u).flatten()
xt_gt[..., i] = x
n_z = np.random.multivariate_normal([0, 0], R)
zt[..., i] = x[[0, 2]] + n_z
# Init Kalman filter
kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=1)
# Assign transition matrices
kf.F = F
kf.H = H
kf.B = B
# Assgin covariance matrices
kf.Q = np.zeros_like(F)
kf.R = R
kf.P = np.diag([16.0, 16.0, 16.0, 16.0])
# Assign init state
kf.x = x_init + np.random.randn(4, 1) * 4
# Run Kalman filter
xt_est = np.empty_like(xt_gt)
for i in range(N):
kf.update(zt[..., i].reshape(2, 1))
kf.predict(u)
kl2_predict = np.zeros((N, 2))
kl2_estimate = np.zeros((N, 2))
kl_predict = np.zeros((N, 2))
kl_estimate = np.zeros((N, 2))
# Initialize:
kl2_predict[0] = init_vec
kl_predict[0] = init_vec
kl2.x = init_vec
kl2.F = F
kl2.H = np.eye(2)
kl2.P = P
kl2.R = R
kl2.Q = Q
kl2.B = B.ravel()
print(kl2.Q)
for i in range(1, N):
kl2.predict(input_f[i - 1] / m_train)
kl2_predict[i] = kl2.x.ravel()
kl2.update(train_measured[i])
kl2_estimate[i] = kl2.x.ravel()
kl_predict[i] = kl.predict(input_f[i - 1] / m_train)
kl_estimate[i] = kl.update(train_measured[i])
_, ax = plt.subplots(figsize=(12, 9))
ax.scatter(times, train_measured_x, marker='+')
ax.plot(times, x_train, lw=1, c='black')
ax.plot(times, kl2_predict[:, 0], label='predict')
x_est = [] # pos x
y_est = [] # pos y
vx_est = [] # vel x
vy_est = [] # vel y
x_est.append(0)
y_est.append(0)
vx_est.append(0)
vy_est.append(0)
my_filter = KalmanFilter(dim_x=4, dim_z=2)
my_filter.x = np.array([[0.], [0.], [0.], [0.]])
my_filter.B = np.array([[(delta_t**2) / 2, 0], [0, (delta_t**2) / 2],
[delta_t, 0], [0, delta_t]])
my_filter.F = np.array([[1., 0., delta_t, 0.], [0., 1., 0., delta_t],
[0., 0., 1., 0.], [0., 0., 0.,
1.]]) # state transition matrix
my_filter.H = np.array([[1., 0., 0., 0.], [0., 1., 0.,
0.]]) # Measurement function
my_filter.P = np.array(
[
[100., 0., 0., 0.],
[0., 100., 0., 0.],
[0., 0., 100., 0.],
[0., 0., 0., 100.],
]
powerrec = np.array([])
u_array = np.array([[0], [0], [0], [0]])
ref_array = np.array([[0], [0], [0], [0]])
integral_array = np.array([[0], [0], [0], [0]])
ucontrolrec = np.array([])
roll_ref = 0
pitch_ref = 0
gpsd = None
## Kalman Filter Initialization
kf = KalmanFilter(dim_x=4, dim_z=4)
kf.x = np.array([[0], [0], [0], [0]])
kf.F = np.array([[1, 0, 0, 0], [DT, 1, 0, 0], [0, 0, 1, 0], [0, 0, DT, 1]])
kf.B = np.array([[0.0001, -0.0001, -0.0001, 0.0001],
[0.0000007, -0.0000007, -0.0000007, 0.0000007],
[0.0001, 0.0001, -0.0001, -0.0001],
[0.0000007, 0.0000007, -0.0000007, -0.0000007]])
kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.P = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
kf.R = np.array([[200, 0, 0, 0], [0, 1000, 0, 0], [0, 0, 200, 0],
[0, 0, 0, 1000]]) # default is 1000, 10000
klqr1 = 20 # default 20
klqr2 = 40 # default 80
klqr = np.array([[klqr1, klqr2, klqr1, klqr2], [-klqr1, -klqr2, klqr1, klqr2],
[-klqr1, -klqr2, -klqr1, -klqr2],
[klqr1, klqr2, -klqr1, -klqr2]])
## Functions & Classes
import psycopg2
import yaml
conn = psycopg2.connect(database="track", user="******",
password="******", host="172.17.0.3",
port="5432")
cur = conn.cursor()
dt = 1
u_noise = 0.010081 #standard deviation
my_filter = KalmanFilter(dim_x=4, dim_z=2)
my_filter.x = np.array([[0.], [0.], [0.], [0.]])
my_filter.B = np.array([[(dt ** 2) / 2], [(dt ** 2) / 2], [dt], [dt]])
my_filter.F = np.array([[1., 0., dt, 0.],
[0., 1., 0., dt],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) # state transition matrix
my_filter.H = np.array([[1., 0., 0., 0.],
[0., 1., 0., 0.]]) # Measurement function
my_filter.P = np.array([[3., 0., 0., 0.],
[0., 3., 0., 0.],
[0., 0., 3., 0.],
[0., 0., 0., 3.]]) # covariance matrix. The terms along the main diagonal of P are the variances associated with the corresponding terms
# in the state vector. The off-diagonal terms of P provide the covariances between terms in the state vector
my_filter.R = [[1.*(3.**2), 0.],
from filterpy.kalman import KalmanFilter
import numpy as np
from filterpy.common import Q_discrete_white_noise
import matplotlib.pyplot as plt
from kf_book.book_plots import plot_measurements
from kf_book.book_plots import plot_filter
from scipy.linalg import block_diag
import TEMP as tm
dt = 2
kf = KalmanFilter(dim_x=2, dim_z=2, dim_u=1)
kf.P = np.array([[1, 0], [0, 1]])
kf.R = 0.05
kf.Q = np.array([[0.001, 0], [0, 0.003]])
kf.F = np.array([[1, -1 * dt], [0, 1]])
kf.B = np.array([[dt], [0]])
kf.H = np.array([[1, 0], [0, 1]])
z_ang, z_vel, zs_ang, zs_vel = tm.Issue_1(50)
zs = []
z = []
for i in range(len(z_ang)):
zs.append([zs_ang[i], 0.01])
z.append([z_ang[i], 0.01])
ut = np.array(z_vel).reshape(-1, 1)
zs = np.array(zs)
z = np.array(z)
result = []
for i in range(len(zs)):
plt.plot(xs, ys)
plt.title('Measurements')
plt.xlabel('x')
plt.ylabel('y')
f2.show()
# Basic Kalman filter
# https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
# 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')
