def get_rk():
global rk
rk = ExtendedKalmanFilter(dim_x=9, dim_z=3)
dt = 0.01
rk.F = asarray([[1., dt, dt * dt / 2, 0., 0., 0., 0., 0., 0.],
[0., 1., dt, 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 1., dt, dt * dt / 2, 0., 0., 0.],
[0., 0., 0., 0., 1., dt, 0., 0., 0.],
[0., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 1., dt, dt * dt / 2],
[0., 0., 0., 0., 0., 0., 0., 1.0, dt],
[0., 0., 0., 0., 0., 0., 0., 0., 1.0]])
# 初始位置 先设置为0
# rk.x = array([0., 0., 1., 0., -0.473, 1.0, 0.6, 0., 1.]).T
rk.x = array([0., 0., 1.0, 0., 0, 1.0, 0.9, 0.0, 0.0]).T
# 测量误差
rk.R *= 0.001
rk.P *= 1
rk.P[6, 6] = 0.0
rk.P[7, 7] = 0.0
rk.P[8, 8] = 0.00
rk.Q *= 0.001
rk.Q[6, 6] = 0.0
rk.Q[7, 7] = 0.0
rk.Q[8, 8] = 0.0
return rk
def __init__(self):
self.wx = 100
self.wy = 0
self.wz = 0
self.q1 = 0
self.q2 = 1
self.q3 = 0
self.q4 = 0
self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2)
def test_initialization_wrong_gamma(self):
with self.assertRaises(ValueError):
filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6)
gamma = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
attacker = PeriodAttacker(filter = self.filter,
radar = self.radar, radar_pos = self.radar_pos,
gamma = gamma,mag_vector = self.mag_vector,
t0 = self.t0, time = self.time)
def test_vhartman():
"""
Code provided by vhartman on github #172
https://github.com/rlabbe/filterpy/issues/172
"""
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
kf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([0.]) # initial state
kf.P = np.array([[1]]) # initial uncertainty
kf.R = np.diag([1]) # 1 standard
kf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
kf.predict()
ekf.predict()
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
kf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, kf.P)
assert np.allclose(ekf.x, kf.x)
def get_rk(self):
self.rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
dt = 0.01
self.rk.F = asarray([[1., dt, dt * dt / 2], [0., 1., dt], [0., 0.,
1.]])
# 初始位置 先设置为0
# self.rk.x = array([0, 0, 0.2]).T
self.rk.x = self.initialState
# 测量误差
self.rk.R *= 0.01
self.rk.P *= 10
self.rk.Q *= 0.001
return self.rk
def setUp(self):
# Simulated filter for 2 radars (2*3 measurements)
self.filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6)
self.gamma = np.eye(3)
self.mag_vector = np.array([[-10, -10, -10]]).T
self.t0 = 10
self.time = 50
self.radar = Radar(x=10,y=10)
self.radar_pos = 1
self.attacker = PeriodAttacker(filter = self.filter,
radar = self.radar, radar_pos = self.radar_pos,
gamma = self.gamma,mag_vector = self.mag_vector,
t0 = self.t0, time = self.time)
def __init__(self, starting_value):
self._filter = ExtendedKalmanFilter(dim_x=2, dim_z=1)
self._filter.x = np.array([starting_value, starting_value])
self._filter.F = np.array([[1.,1.],
[0.,1.]])
self._filter.H = np.array([[1.,0.]])
self._filter.P = np.array([[1000., 0.],
[ 0., 1000.] ])
self._filter.R = np.array([[5.]])
self._filter.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
self.inc = 0
self.last_val = starting_value
self.first = True
def __init__(self):
self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2)
self.wx = 0
self.wy = 0
self.wz = 265
self.q1 = 1
self.q2 = 0
self.q3 = 0
self.q4 = 0
#range_std = 5000
self.rk.R = np.diag([300,500])
self.rk.Q = np.diag([50,50,50,50,0.1,0.1,0.1])
self.rk.P *= 50
self.rk.x = np.array([[self.q1,self.q2,self.q3,self.q4,self.wx,self.wy,self.wz]]).T
def initFilter(app):
app.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1)
# The estimate
app.f.x = np.array([[1.], [1.]])
# State transition matrix, just use last position for now
app.f.F = np.array([[1., 0.], [0., 1.]])
# Measurement function, values from linear localization
app.f.P = np.eye(
2) * 1000 # Covaraince, large number since our initial guess is (1, 1)
app.f.R = np.eye(
len(ANCHORS) - 1
) * 50 # Measurement uncertainty, making it low since we want to weight sensor data
app.f.Q = np.eye(
2
) * 1 # System noise, making it big since we're just using the last location as our prediction
def get_rk():
global rk
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
dt = 0.01
rk.F = asarray([[1., dt, dt * dt / 2],
[0., 1., dt],
[0., 0., 1.],
])
# 初始位置 先设置为0
rk.x = array([0, 0, 0]).T
# 测量误差
rk.R *= 0.001
rk.P *= 0.1
rk.Q *= 0.001
return rk
def __init__(self):
if (VELOCITY):
self.f = ExtendedKalmanFilter(dim_x=4,
dim_z=(len(ANCHORS) - 1 + 2))
# The estimate
self.f.x = np.array([[1.], [1.], [0.], [0.]])
# State transition matrix, just use last position for now
self.f.F = np.array([[1., 0., 0.12, 0.], [0., 1., 0., 0.12],
[0., 0., 1., 0.], [0., 0., 0., 1.]])
# Measurement function, values from linear localization
self.f.P = np.eye(
4
) * P # Covaraince, large number since our initial guess is (1, 1)
self.f.R = np.eye(
len(ANCHORS) - 1 + 2
) * R # Measurement uncertainty, making it high since noisy data
self.f.Q = np.eye(
4
) * Q # System noise, making it small since we're using a velocity estimation
else:
self.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1)
# The estimate
self.f.x = np.array([[1.], [1.]])
# State transition matrix, just use last position for now
self.f.F = np.array([[1., 0.], [0., 1.]])
# Measurement function, values from linear localization
self.f.P = np.eye(
2
) * P # Covaraince, large number since our initial guess is (1, 1)
self.f.R = np.eye(
len(ANCHORS) - 1
) * R # Measurement uncertainty, making it low since we want to weight sensor data
self.f.Q = np.eye(
2
) * Q # System noise, making it big since we're just using the last location as our prediction
def __init__(self, path):
dt = 0.05
self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
# radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
#self.detector = PlanetDetector('C:\\Users\\Phuc Dang\\Desktop\\Developer\\Collaborative-Sensing\\model\\cascade.xml')
self.detector = PlanetDetector(path)
# make an imperfect starting guess
# rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000])
self.rk.x = array([0, .273]) # need better initial location
self.rk.F = eye(2) + array([[0, 0], [1, 0]]) * dt
range_std = 5. # meters
self.rk.R = np.diag([range_std**2])
self.rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
self.rk.Q[1, 1] = 0.1
self.rk.P *= 50
def test_ukf_ekf_comparison():
def fx(x, dt):
# state transition function - predict next state based
# on constant velocity model x = vt + x_0
F = np.array([[1.]], dtype=np.float32)
return np.dot(F, x)
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
return np.array([x[0]])
dt = 1.0
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(1, alpha=1, beta=2., kappa=0.1)
ukf = UnscentedKalmanFilter(dim_x=1,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
ukf.x = np.array([0.]) # initial state
ukf.P = np.array([[1]]) # initial uncertainty
ukf.R = np.diag([1]) # 1 standard
ukf.Q = np.diag([1]) # 1 standard
ekf = ExtendedKalmanFilter(dim_x=1, dim_z=1)
ekf.F = np.array([[1]])
ekf.x = np.array([0.]) # initial state
ekf.P = np.array([[1]]) # initial uncertainty
ekf.R = np.diag([1]) # 1 standard
ekf.Q = np.diag([1]) # 1 standard
np.random.seed(0)
zs = [[np.random.randn()] for i in range(50)] # measurements
for z in zs:
ukf.predict()
ekf.predict()
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after prediction'
ukf.update(z)
ekf.update(z, lambda x: np.array([[1]]), hx)
assert np.allclose(ekf.P, ukf.P), 'ekf and ukf differ after update'
def get_rk():
global rk
global rotating_model_jacobian
rotating_model_jacobian = Rotating_Model_Jacobian()
rk = ExtendedKalmanFilter(dim_x=7, dim_z=3)
dt = 0.01
rk.F = asarray([[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.],
[0., 0., 0., 0., 1., dt, dt * dt / 2],
[0., 0., 0., 0., 0., 1., dt], [0., 0., 0., 0., 0., 0.,
1.]])
# 初始位置 先设置为0
rk.x = array([1, 1, 1, 0.4, 0, 1, 1]).T
# 测量误差
rk.R *= 0.001
rk.P *= 1
rk.Q *= 0.001
return rk
def setUp(self):
# Simulated filter for 2 radars (2*3 measurements)
self.filter = ExtendedKalmanFilter(dim_x=9, dim_z=6)
# Attack matrix: second radar is compromised
self.gamma = np.array([[0, 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, 1, 0], [0, 0, 0, 0, 0, 1]])
self.mag_vector = np.array([[0, 0, 0, -10, -10, -10]]).T
self.t0 = 10
self.time = 50
self.radar = Radar(x=10, y=10)
self.attacker = Attacker(filter=self.filter,
radar=self.radar,
gamma=self.gamma,
mag_vector=self.mag_vector,
t0=self.t0,
time=self.time)
def __init__(self, K, dim_x=3, dim_z=3, lamb=2, sigma=2, dt=0.05):
#Init Kalman filter
self.rk = ExtendedKalmanFilter(dim_x=dim_x, dim_z=dim_z)
self.rk.x = np.array([1, 1, 1])
self.sigma = sigma
self.lamb = lamb
self.K = K
self.rho = 28.0
self.sigma_lorenz = 10.0
self.beta = 8.0 / 3.0
self.dt = dt
self.rk.F = self.get_F(self.rk.x)
self.rk.R = np.diag([lamb**2]*3)
self.rk.Q = self.get_Q(dt=self.dt)
self.rk.H = np.identity(3, dtype=np.float32)
self.rk.P *= 3
def test_saver_ekf():
def HJ(x):
return np.array([[1, 1]])
def hx(x):
return np.array([x[0]])
kf = ExtendedKalmanFilter(2, 1)
s = Saver(kf)
for i in range(3):
kf.predict()
kf.update([[i]], HJ, hx)
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 3
assert len(s.y) == 3
assert len(s.K) == 3
def build_ekf(coeffs, z_data, linear_consts=None, nmsmt=n_msmt, dx=dimx):
global n_msmt
global dimx
(dimx, n_msmt) = (dx, nmsmt)
ekf = ExtendedKalmanFilter(dim_x=dimx, dim_z=n_msmt)
#ekf.x = zeros([dimx, 1])
#ekf.__init__(dimx, n_msmt)
if len(coeffs):
coeffs = array(coeffs).flatten()
if n_coeff == dimx * 2 + n_msmt:
ekf.Q = symmetric(array(coeffs[0:dimx]))
ekf.F = symmetric(array(coeffs[dimx:dimx * 2]))
r = symmetric(array(coeffs[-n_msmt:]))
else:
ekf.Q = read2d(coeffs, dimx, 0, dimx * dimx)
ekf.F = read2d(coeffs, dimx, dimx * dimx, dimx * dimx * 2)
r = read2d(coeffs, n_msmt, -n_msmt * n_msmt, n_coeff)
logger.info("ekf.Q=" + str(ekf.Q) + ", ekf.F = " + str(ekf.F) +
", r = " + str(r))
return update_ekf(ekf, z_data, r, linear_consts)
return update_ekf(ekf, z_data)
def setup():
# set up sensor simulator
F = np.array([[1, dt, 0], [0, 1, 0], [0, 0, 1]])
def f(x):
return np.dot(F, x)
x0 = np.array([[-5], [0.5], [1]])
sim = Sensor(x0, f, h)
# set up kalman filter
tracker = ExtendedKalmanFilter(dim_x=3, dim_z=2)
tracker.F = F
q_x = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
q_y = 0.0001 * dt**2
tracker.Q = block_diag(q_x, q_y)
tracker.R = R_proto * measurement_var_max * filter_misestimation_factor
tracker.x = np.array([[-5, 0.5, 1]]).T
tracker.P = np.eye(3) * 500
return sim, tracker
def __init__(self, planetDetector, FIFO_FILENAME, OTHER_FIFO):
# Haar classifier
self.detector = planetDetector
# EKF model
self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
self.initialize_rk()
# FIFO
self.writeFiFo = os.open(FIFO_FILENAME, os.O_WRONLY)
self.readFiFo = os.open(OTHER_FIFO, os.O_RDONLY)
if FIFO_FILENAME == "srv_transmit":
self.server = True
else:
self.server = False
#Merge values
self.omega = self.rk.x[0]
self.omega_hat = self.rk.x[1]
#Client sensor values
self.omega_client = self.rk.x[0]
self.omega_hat_client = self.rk.x[1]
def kalman_filter(y, fs):
r_locs = detect_qrs(y, fs)
r_vals = y[r_locs]
r_val_mean = r_vals.mean()
y_truncated = y[r_locs[0]:(r_locs[-1] + 1)]
theta_value = 0
theta_init = np.array([-np.pi / 3, -np.pi / 12, 0, np.pi / 12, np.pi / 2])
a_init = np.array([1.2, -5.0, 30.0, -7.5, 0.75])
b1_init = np.array([0.25, 0.1, 0.1, 0.1, 0.4])
b2_init = np.array([0.25, 0.1, 0.1, 0.1, 0.4])
a_init = a_init * r_val_mean / 30
x_init = np.concatenate(
[a_init, theta_init, b1_init, b2_init,
np.array([y_truncated[0]])])
val_init = np.concatenate(
[a_init, theta_init, b1_init, b2_init,
np.array([theta_value])])
d_theta = 0.005 * 2 * np.pi
rk = ExtendedKalmanFilter(dim_x=21, dim_z=1)
rk.x = np.array([x_init]).T
rk.F = eye(21)
rk.R = np.diag([0.1])
rk.Q = np.diag([0.1 for _ in range(21)])
val_v = val_init
xs = []
for i in tqdm(range(len(y_truncated))):
z = y_truncated[i]
for j in range(20):
rk.F[20, j] = eval_var(ag_d_theta_var_d_params[j], val_v)
rk.update(array([[z]]), HJacobian_at, hx)
xs.append(rk.x)
rk.predict()
val_v[20] += d_theta
if val_v[20] > np.pi:
val_v[20] -= 2 * np.pi
val_v[:20] = rk.x[0, :20]
return np.array(xs)[:, 20, 0]
#
d0 = 10
def HJacobian_at(x):
angular_vel = x[1]
angular_pos = x[0]
return array([[0., (-1)*np.sin(angular_pos)*d0]])
def get_pixel_between_sun_and_planet(x):
return d0 * cos(x[0])
dt = 0.05
rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
#radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
detector = PlanetDetector('../old_models/model_4/cascade.xml', False, False)
# make an imperfect starting guess
#rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000])
rk.x = array([0, 0.237]) #need better initial location
rk.F = eye(2) + array([[0, 0],
[1, 0]]) * dt
range_std = 5. # meters
bs = np.diag([range_std ** 2])
print(bs)
def __init__(self):
self.ekf = ExtendedKalmanFilter(dim_x=6, dim_z=3)
self.set_pose(np.zeros(6))
self.dt = 1
def eval_segpose_icp_kalman_predictions(image_ids=None,
object_names=None,
selected_noise=param.selected_noise):
if image_ids is None:
image_ids = param.selected_linemod_image_ids
if object_names is None:
object_names = list(param.selected_linemod_objects.keys())
model_clouds = io.load_clouds_from_selected_models(True, object_names)
ds_avg = np.zeros((len(object_names), len(image_ids), 5))
ds_std = np.zeros((len(object_names), len(image_ids), 5))
images = []
depths = []
for i in image_ids:
image, depth = io.load_rgb_and_d(i,selected_noise=selected_noise)
images.append(image)
depths.append(depth)
# First compute the image to image transformations
intrinsic = io.load_camera_intrinsics_open3d().intrinsic_matrix
ans = optical_flow.compute_tracked_features_and_tranformation(
images, depths, intrinsic)
image_RTs = ans[:2]
err = ans[2]
def hx(x,image_RT):
x_shape = x.shape
x = np.ndarray.flatten(x)
RT_x = np.eye(4)
RT_x[:3, :3] = R.from_euler('xyz', x[3:]).as_matrix()
RT_x[:3, -1] = x[:3]
x_pred = image_RT @ RT_x
# Convert back to euler
euler_angles = R.from_matrix(x_pred[:3, :3]).as_euler('xyz')
state = np.hstack([x_pred[:3, -1], euler_angles])
return np.reshape(state, x_shape)
def H(x):
return np.eye(6)
# Set up an Extended Kalman filter. Assume we are observing the 6D pose.
ekf = ExtendedKalmanFilter(6, 6) # xyz, euler
prev_RT_kalman_input = None
for obj_i, object_name in enumerate(object_names):
for img_i, image_id in enumerate(image_ids):
scene_cloud = io.load_cloud_from_selected_image_id(image_id,
selected_noise=selected_noise,
intrinsics=intrinsic)
RT_gt = np.eye(4)
RT_segpose = np.eye(4)
RT_gt[:3,:] = io.load_ground_truth_pose(object_name,image_id)
RT_segpose[:3,:] = io.load_segpose_prediction(object_name,image_id,
selected_noise=selected_noise)
# compute ICP using segpose prediction as the seed
threshold = 0.1
reg_p2p = o3d.pipelines.registration.registration_icp(
model_clouds[object_name], scene_cloud, threshold, RT_segpose)
RT_est = reg_p2p.transformation
# update Kalman filter
RT_kalman_input = RT_segpose
image_transformation = np.eye(4)
if img_i > 0:
image_transformation[:3, :3] = image_RTs[0][img_i-1]
image_transformation[:3, -1] = image_RTs[1][img_i-1]
if prev_RT_kalman_input is None:
euler_angles = R.from_matrix(RT_kalman_input[:3, :3]).as_euler('xyz')
state_observed = np.hstack([RT_kalman_input[:3, -1], euler_angles])
ekf.predict_update(np.reshape(state_observed, (-1, 1)), H, hx,
hx_args=(np.eye(4),))
# Do EKF as euler angles
# Compute the estimation
euler_angles = R.from_matrix(RT_kalman_input[:3,:3]).as_euler('xyz')
state_observed = np.hstack([RT_kalman_input[:3,-1], euler_angles])
ekf.predict_update(np.reshape(state_observed,(-1,1)), H, hx,
hx_args=(image_transformation,))
state_post = np.ndarray.flatten(ekf.x_post)
RT_kalman = np.eye(4)
RT_kalman[:3,:3] = R.from_euler('xyz',state_post[3:]).as_matrix()
RT_kalman[:3,-1] = state_post[:3]
state_prior = np.ndarray.flatten(ekf.x_prior)
RT_kalman_prior = np.eye(4)
RT_kalman_prior[:3,:3] = R.from_euler('xyz',state_prior[3:]).as_matrix()
RT_kalman_prior[:3,-1] = state_prior[:3]
reg_kalman = o3d.pipelines.registration.registration_icp(
model_clouds[object_name], scene_cloud, threshold, RT_kalman)
RT_kalman_icp = reg_kalman.transformation
# Memoize the previous RT estimation from point cloud & segpose
prev_RT_kalman_input = np.copy(RT_kalman_input)
model_cloud_gt = copy.copy(model_clouds[object_name])
model_cloud_gt.transform(RT_gt)
model_cloud_seg = copy.copy(model_clouds[object_name])
model_cloud_seg.transform(RT_segpose)
model_cloud_est = copy.copy(model_clouds[object_name])
model_cloud_est.transform(RT_est)
model_cloud_kalman = copy.copy(model_clouds[object_name])
model_cloud_kalman.transform(RT_kalman)
model_cloud_kalman_icp = copy.copy(model_clouds[object_name])
model_cloud_kalman_icp.transform(RT_kalman_icp)
model_cloud_kalman_prior = copy.copy(model_clouds[object_name])
model_cloud_kalman_prior.transform(RT_kalman_prior)
print(img_i)
for cloud_i, cloud in enumerate([model_cloud_seg,
model_cloud_est,
model_cloud_kalman,
model_cloud_kalman_icp]):
d_current = np.asarray(model_cloud_gt.compute_point_cloud_distance(cloud))
ds_avg[obj_i, img_i, cloud_i] = np.average(d_current)
ds_std[obj_i, img_i, cloud_i] = np.std(d_current)
# visualize.draw_point_clouds([model_cloud_gt, model_cloud_seg,
# model_cloud_est, model_cloud_kalman,
# model_cloud_kalman_icp,
# scene_cloud],
# [[1,0,0],[0,1,0],[0,0,1],
# [1,1,0],[1,0,1]]) #R: GT, G: Seg, B: Seg+ICP, Y:Kalman, P: Kalman+ICP
return ds_avg, ds_std
return array(jacobian)
def hx(x):
ranges = []
est_x = x.item(0)
est_y = x.item(1)
for s in radar.sensors:
ranges.append([sqrt((est_x - s[0])**2 + (est_y - s[1])**2)])
return ranges
radar = RadarNet()
ekf = ExtendedKalmanFilter(dim_x=2, dim_z=3)
ekf.x = array([[1.0], [1.0]])
# ekf.x = array([[radar.pos, radar.vel+100, radar.alt+1000]]).T
# ekf.x = array([[0.0, 0.0, 0.0]]).T
ekf.F = eye(2)
# ekf.R = radar.target[1] * 0.05
ekf.R *= 10
ekf.Q *= 0.0001
ekf.P *= 50
truth_x = []
truth_y = []
estimated_x = []
return slant_dist + err
def HJacobian_at (x):
horiz_dist = x[0,0]
altitude = x[2,0]
denom = sqrt(horiz_dist**2 + altitude**2)
return array([[horiz_dist/denom, 0.0, altitude/denom]])
def hx (x):
return (x[0,0]**2 + x[2,0]**2)**0.5
dt = 0.05
flight = Flight (dt, pos = 0.0, vel = 100.0, alt = 1000.0)
ekf = ExtendedKalmanFilter (dim_x = 3, dim_z = 1)
ekf.x = array([[flight.pos, flight.vel+100, flight.alt+1000]]).T
# ekf.x = array([[flight.pos, flight.vel+100, flight.alt+1000]]).T
# ekf.x = array([[0.0, 0.0, 0.0]]).T
ekf.F = eye(3) + array([[0,1,0], [0,0,0], [0,0,0]])*dt
ekf.R = flight.alt * 0.05
ekf.Q = array ([[0,0,0], [0,1,0], [0,0,1]]) * 0.001
ekf.P *= 50
truth_x = []
truth_y = []
estimated_x = []
estimated_y = []
# Functionality :
# ###################################
from scipy.io import loadmat
from numpy import eye, array
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from filterpy.kalman import ExtendedKalmanFilter
noisy_data = loadmat('l10_y.mat')['y']
clean_data = loadmat('l10_x_true.mat')['x_true']
noisy_data.shape
EKF = ExtendedKalmanFilter(1, 1)
EKF.x = 0
dt = 0.05
EKF.F = eye(3) + array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) * dt
EKF.R
def HJacobian_at(x):
new_signal = (1.9999 * x[i - 1]) + (-1 * x[i - 2])
return new_signal
#EKF.y = noisy_data
#print(EKF.x_post)
#something = EKF.update(None,'HJacobian',clean_data)
def __init__(self, cameras, keypoints, R_std, Q_var):
#Variables:
self.camera_number = cameras # number ob cameras
self.state_dim = 6 # per keypoint
self.measurement_dim = 2 * self.camera_number # per keypoint
self.keypoint_number = keypoints # COCO model
self.frame_number = 0 # actual interation number of the filter
self.R_std = R_std**2 # measurement noise: before 1.0
self.Q_var = Q_var**2 # process/system noise: before 5.0
# state dimension: [x,y,z,x',y',z'] for 18 keypoints
self.x_dimension = self.state_dim * self.keypoint_number # 108
# measurement dimension: [x0, y0, x1, y1, x2,..., x_j, y_j] screen coord. for all j cameras for 18 keypoints
self.z_dimension = self.measurement_dim * self.keypoint_number # 180
self.dt = 0.04 # time steps: 1/25 FPS
#Build State Vector X: [x,y,z,x',y',z'] for each keypoint
X = np.zeros((self.x_dimension, 1))
#Create the actual filter now:
ekf = ExtendedKalmanFilter(dim_x=self.x_dimension,
dim_z=self.z_dimension)
# state vector
ekf.x = X
# state covariance
#ekf.P = np.zeros((self.x_dimension,self.x_dimension))
ekf.P = np.eye(self.x_dimension) * 20
# Process Model
# Build Transition Matrix F or also called A
block = np.matrix([[1., 0., 0., self.dt, 0., 0.],
[0., 1., 0., 0., self.dt, 0.],
[0., 0., 1., 0., 0., self.dt],
[0., 0., 0., 1., 0., 0.], [0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 1.]])
matrix_list = []
for i in range(self.keypoint_number):
matrix_list.append(block)
F = scipy.linalg.block_diag(*matrix_list)
ekf.F = F
# measurement noise
ekf.R = np.eye(self.z_dimension) * (self.R_std)
# process noise
# TODO: which noise model should be used?
# q = Q_discrete_white_noise(dim=2, dt=self.dt, var=self.Q_var)
# block = np.matrix([[q[0,0], 0., 0., q[0,1], 0., 0.],
# [0., q[0,0], 0., 0., q[0,1], 0.],
# [0., 0., q[0,0], 0., 0., q[0,1]],
# [q[1,0], 0., 0., q[1,1], 0., 0.],
# [0., q[1,0], 0., 0., q[1,1], 0.],
# [0., 0., q[1,0], 0., 0., q[1,1]]])
# matrix_list = []
# for i in range (self.keypoint_number):
# matrix_list.append(block)
# ekf.Q = scipy.linalg.block_diag(*matrix_list)
ekf.Q = np.eye(self.x_dimension) * (self.Q_var)
self.filter = ekf
def extended_kalman_filter():
from filterpy.kalman import ExtendedKalmanFilter
data_array = np.load('/media/samsumg_1tb/synthia/prepared_data_shuffle.npy')
test_data_array = data_array[2]
data_mean = data_array[3]
data_std = data_array[4]
test_data_array = (test_data_array * data_std) + data_mean
print('Finish Loading. Test array shape: ' + str(test_data_array.shape))
test_pred = np.zeros(shape=(len(test_data_array), 8, 6))
test_pred[:, :, 4:] = test_data_array[:, -8:, 4:]
def HJacobian_at(x):
""" compute Jacobian of H matrix at x """
return np.array([[0, 1., 0, 1]])
def hx(x):
""" compute measurement for slant range that
would correspond to state x.
"""
return x
for f, data in enumerate(test_data_array):
if f % 100 == 0:
print("process batch %d" %f)
measurements = [[d[0], d[1]] for d in data[:15]]
ekf = ExtendedKalmanFilter(dim_x=4, dim_z=2)
ekf.x = np.array([measurements[0][0], measurements[0][1], 0, 0])
ekf.F = np.array([[1., 0., 1., 0.],
[0., 1., 0., 1.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
# Define the measurement function:
ekf.H = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.]])
# Define the covariance matrix. Here I take advantage of the fact that P already contains np.eye(dim_x),
# and just multiply by the uncertainty:
ekf.P *= 1
for t in range(1, 15):
ekf.predict()
ekf.update([measurements[t][0], measurements[t][1]], HJacobian_at, hx)
for t in range(8):
ekf.predict()
next_x = ekf.x
test_pred[f, t, 0] = next_x[0]
test_pred[f, t, 1] = next_x[1]
test_pred[f, t, 2] = test_data_array[f, 14, 2] # we set the width to be fixed
test_pred[f, t, 3] = test_data_array[f, 14, 3] # we set the width to be fixed
totalerrCoverage = 0
totalerrCenter = 0
Kalman_valid_errCenter = []
Kalman_valid_iou_2d = []
for i in range(len(test_pred)):
res = test_pred[i]
anno = test_data_array[i, -8:, :]
center = [[r[0], r[1]] for r in res]
centerGT = [[r[0], r[1]] for r in anno]
seq_length = len(centerGT)
errCenter = [ssd_2d(center[i], centerGT[i]) for i in range(len(centerGT))]
Kalman_valid_errCenter.append(errCenter)
iou_2d = calc_rect_int_2d(res, anno)
Kalman_valid_iou_2d.append(iou_2d)
for s in range(seq_length):
totalerrCenter += errCenter[s]
totalerrCoverage += iou_2d[s]
aveErrCoverage = totalerrCoverage / (len(Kalman_valid_errCenter) * float(seq_length))
aveErrCenter = totalerrCenter / (len(Kalman_valid_errCenter) * float(seq_length))
print('aveErrCoverage: %.4f, aveErrCenter: %.2f ' %(aveErrCoverage, aveErrCenter))
# P: aveErrCoverage: 0.6174, aveErrCenter: 31.11
result_dir = '/media/samsumg_1tb/cvpr_DTA_Results/kalman_filter'
np.save(os.path.join(result_dir, 'Kalman_valid_errCenter.npy'), Kalman_valid_errCenter)
np.save(os.path.join(result_dir, 'Kalman_valid_iou_2d.npy'), Kalman_valid_iou_2d)
def test_ekf():
def H_of(x):
""" compute Jacobian of H matrix for state x """
horiz_dist = x[0]
altitude = x[2]
denom = sqrt(horiz_dist**2 + altitude**2)
return array([[horiz_dist / denom, 0., altitude / denom]])
def hx(x):
""" takes a state variable and returns the measurement that would
correspond to that state.
"""
return sqrt(x[0]**2 + x[2]**2)
dt = 0.05
proccess_error = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
rk.F = eye(3) + array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) * dt
def fx(x, dt):
return np.dot(rk.F, x)
rk.x = array([-10., 90., 1100.])
rk.R *= 10
rk.Q = array([[0, 0, 0], [0, 1, 0], [0, 0, 1]]) * 0.001
rk.P *= 50
rs = []
xs = []
radar = RadarSim(dt)
ps = []
pos = []
s = Saver(rk)
for i in range(int(20 / dt)):
z = radar.get_range(proccess_error)
pos.append(radar.pos)
rk.update(asarray([z]), H_of, hx, R=hx(rk.x) * proccess_error)
ps.append(rk.P)
rk.predict()
xs.append(rk.x)
rs.append(z)
s.save()
# test mahalanobis
a = np.zeros(rk.y.shape)
maha = scipy_mahalanobis(a, rk.y, rk.SI)
assert rk.mahalanobis == approx(maha)
s.to_array()
xs = asarray(xs)
ps = asarray(ps)
rs = asarray(rs)
p_pos = ps[:, 0, 0]
p_vel = ps[:, 1, 1]
p_alt = ps[:, 2, 2]
pos = asarray(pos)
if DO_PLOT:
plt.subplot(311)
plt.plot(xs[:, 0])
plt.ylabel('position')
plt.subplot(312)
plt.plot(xs[:, 1])
plt.ylabel('velocity')
plt.subplot(313)
#plt.plot(xs[:,2])
#plt.ylabel('altitude')
plt.plot(p_pos)
plt.plot(-p_pos)
plt.plot(xs[:, 0] - pos)
