def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
sp = JulierSigmaPoints(1, kappa)
#ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
Wm, Wc = sp.weights()
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 3
mean = 5
cov = 9
Xi = sp.sigma_points (mean, cov)
xm, ucov = unscented_transform(Xi,Wm, Wc, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, Wm):
m += x*w
assert abs(m-mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0,0]-cov) < 1.e-12
assert Xi.shape == (3,1)
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
sp = JulierSigmaPoints(1, kappa)
#ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
Wm, Wc = sp.weights()
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 3
mean = 5
cov = 9
Xi = sp.sigma_points(mean, cov)
xm, ucov = unscented_transform(Xi, Wm, Wc, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, Wm):
m += x * w
assert abs(m - mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0, 0] - cov) < 1.e-12
assert Xi.shape == (3, 1)
def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2], [1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
Xi0 = sp0.sigma_points(x, P)
Xi1 = sp1.sigma_points(x, P)
assert max(Xi1[:, 0]) > max(Xi0[:, 0])
assert max(Xi1[:, 1]) > max(Xi0[:, 1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0],
(Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1],
color='blue')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0],
(Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1],
color='green')
stats.plot_covariance_ellipse([1, 2], P)
def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2], [1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa * 1000)
sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3)
sp3 = SimplexSigmaPoints(n=2)
# test __repr__ doesn't crash
str(sp0)
str(sp1)
str(sp2)
str(sp3)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
w2, _ = sp2.weights()
w3, _ = sp3.weights()
Xi0 = sp0.sigma_points(x, P)
Xi1 = sp1.sigma_points(x, P)
Xi2 = sp2.sigma_points(x, P)
Xi3 = sp3.sigma_points(x, P)
assert max(Xi1[:, 0]) > max(Xi0[:, 0])
assert max(Xi1[:, 1]) > max(Xi0[:, 1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i, 0] - x[0, 0]) * w0[i] + x[0, 0],
(Xi0[i, 1] - x[0, 1]) * w0[i] + x[0, 1],
color='blue',
label='Julier low $\kappa$')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0] - x[0, 0]) * w1[i] + x[0, 0],
(Xi1[i, 1] - x[0, 1]) * w1[i] + x[0, 1],
color='green',
label='Julier high $\kappa$')
# for i in range(Xi2.shape[0]):
# plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0],
# (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1],
# color='red')
for i in range(Xi3.shape[0]):
plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0],
(Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1],
color='black',
label='Simplex')
stats.plot_covariance_ellipse([1, 2], P)
def build_ukf(x0=None, P0=None,
Q = None, R = None
):
# build ukf
if x0 is None:
x0 = np.zeros(6)
if P0 is None:
P0 = np.diag([1e-6,1e-6,1e-6, 1e-1, 1e-1, 1e-1])
if Q is None:
Q = np.diag([1e-4, 1e-4, 1e-2, 1e-1, 1e-1, 1e-1]) #xyhvw
if R is None:
R = np.diag([1e-1, 1e-1, 1e-1]) # xyh
#spts = MerweScaledSigmaPoints(6, 1e-3, 2, 3-6, subtract=ukf_residual)
spts = JulierSigmaPoints(6, 6-2, sqrt_method=np.linalg.cholesky, subtract=ukf_residual)
ukf = UKF(6, 3, (1.0 / 30.), # dt guess
ukf_hx, ukf_fx, spts,
x_mean_fn=ukf_mean,
z_mean_fn=ukf_mean,
residual_x=ukf_residual,
residual_z=ukf_residual)
ukf.x = x0.copy()
ukf.P = P0.copy()
ukf.Q = Q
ukf.R = R
return ukf
def __init__(self, dt, state=np.array([0.0, 0.0, -2.0, 0.0, 0.0, 0.0])):
M = len(state)
points = JulierSigmaPoints(M)
def project(state, world_positions):
return project_plane_markers(world_positions,
state.reshape(2, 3)).reshape(-1)
self.kf = kf = UnscentedKalmanFilter(
dt=dt,
dim_x=M,
dim_z=1,
points=points,
#fx=lambda X, dt: predict_state(X.reshape(4, -1), dt).reshape(-1),
fx=lambda x, dt: x,
hx=project,
)
z_dim = 2 # This changes according to the measurement
kf.P = np.eye(M) * 0.3
kf.x = state.reshape(-1).copy() # Initial state guess
self.z_var = 0.05**2
#kf.R = z_var # Observation variance
dpos_var = 0.01**2 * dt
drot_var = np.radians(1.0)**2 * dt
#kf.Q = np.diag([0]*3 + [0]*3 + [dpos_var]*3 + [drot_var]*3)
kf.Q = np.diag([dpos_var] * 3 + [drot_var] * 3)
def test_rts():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return np.sqrt(x[0]**2 + x[2]**2)
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=1.)
kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
#xs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3) * 100
M, P = kf.batch_filter(rs)
assert np.array_equal(M, xs), "Batch filter generated different output"
Qs = [kf.Q] * len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.plot(t, M2[:, 0], c='g')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.plot(t, M2[:, 1], c='g')
plt.subplot(313)
plt.plot(t, xs[:, 2])
plt.plot(t, M2[:, 2], c='g')
def sensor_fusion_kf():
global hx, fx
# create unscented Kalman filter with large initial uncertainty
points = JulierSigmaPoints(2, kappa=2)
kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points)
kf.x = np.array([100, 100.])
kf.P *= 40
return kf
def test_radar():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])
return A.dot(x)
def hx(x):
return [np.sqrt(x[0]**2 + x[2]**2)]
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0.)
kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20+dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
for i in range(len(t)):
r = radar.get_range()
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
# test mahalanobis
a = np.zeros(kf.y.shape)
maha = scipy_mahalanobis(a, kf.y, kf.SI)
assert kf.mahalanobis == approx(maha)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.subplot(313)
plt.plot(t, xs[:, 2])
def test_radar():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
return A.dot(x)
def hx(x):
return np.sqrt(x[0]**2 + x[2]**2)
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0.)
# sp = SimplexSigmaPoints(n=3)
kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
assert np.allclose(kf.x, kf.x_prior)
assert np.allclose(kf.P, kf.P_prior)
# test __repr__ doesn't crash
str(kf)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 100.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
#xs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
rs.append(r)
if DO_PLOT:
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.subplot(313)
plt.plot(t, xs[:, 2])
def __init__(self, env, dim_x, dim_y, X_0, P, Q, R, mn):
# self.sigmas = JulierSigmaPoints(dim_x, alpha=.1, beta=2., kappa=1.)
self.sigmas = JulierSigmaPoints(dim_x, kappa=0.1)
self.env = env
self.measure_nums = mn
self.ukf = UnscentedKalmanFilter(dim_x=dim_x,
dim_z=dim_y,
dt=env.tau,
hx=self.measurement,
fx=UKF_model,
points=self.sigmas)
self.ukf.x = X_0[:, 0]
# print(self.ukf.x.shape)
self.ukf.P = np.copy(P)
self.ukf.R = np.copy(R)
self.ukf.Q = np.copy(Q)
def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2],
[1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)
sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3)
sp3 = SimplexSigmaPoints(n=2)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
w2, _ = sp2.weights()
w3, _ = sp3.weights()
Xi0 = sp0.sigma_points(x, P)
Xi1 = sp1.sigma_points(x, P)
Xi2 = sp2.sigma_points(x, P)
Xi3 = sp3.sigma_points(x, P)
assert max(Xi1[:,0]) > max(Xi0[:,0])
assert max(Xi1[:,1]) > max(Xi0[:,1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0],
(Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1],
color='blue', label='Julier low $\kappa$')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0],
(Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1],
color='green', label='Julier high $\kappa$')
# for i in range(Xi2.shape[0]):
# plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0],
# (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1],
# color='red')
for i in range(Xi3.shape[0]):
plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0],
(Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1],
color='black', label='Simplex')
stats.plot_covariance_ellipse([1, 2], P)
def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2],
[1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
Xi0 = sp0.sigma_points (x, P)
Xi1 = sp1.sigma_points (x, P)
assert max(Xi1[:,0]) > max(Xi0[:,0])
assert max(Xi1[:,1]) > max(Xi0[:,1])
def test_sigma_plot():
""" Test to make sure sigma's correctly mirror the shape and orientation
of the covariance array."""
x = np.array([[1, 2]])
P = np.array([[2, 1.2],
[1.2, 2]])
kappa = .1
# if kappa is larger, than points shoudld be closer together
sp0 = JulierSigmaPoints(n=2, kappa=kappa)
sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000)
w0, _ = sp0.weights()
w1, _ = sp1.weights()
Xi0 = sp0.sigma_points (x, P)
Xi1 = sp1.sigma_points (x, P)
assert max(Xi1[:,0]) > max(Xi0[:,0])
assert max(Xi1[:,1]) > max(Xi0[:,1])
if DO_PLOT:
plt.figure()
for i in range(Xi0.shape[0]):
plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0],
(Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1],
color='blue')
for i in range(Xi1.shape[0]):
plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0],
(Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1],
color='green')
stats.plot_covariance_ellipse([1, 2], P)
def track_head(camera_matrix, markerss, world_markers):
M = camera_matrix
f_x, c_x = M[0, 0], M[0, -1]
f_y, c_y = M[1, 1], M[1, -1]
h = 1
w = 16 / 9
dt = 1 / 30 # TODO: Use the actual timestamps
pos = np.array([0.0, 0.0, -2.0])
rot = np.array([0.0, 0.0, 0.0])
dpos = np.array([0.0, 0.0, 0.0])
drot = np.array([0.0, 0.0, 0.0])
state = np.array([
pos,
rot,
#dpos, drot
])
state_shape = state.shape
M = np.prod(state_shape)
points = JulierSigmaPoints(M)
def project(state, world_positions):
return project_plane_markers(world_positions,
state.reshape(state_shape)).reshape(-1)
kf = UnscentedKalmanFilter(
dt=dt,
dim_x=M,
dim_z=len(world_markers) * 2,
points=points,
#fx=lambda X, dt: predict_state(X.reshape(4, -1), dt).reshape(-1),
fx=lambda x, dt: x,
hx=project,
)
z_dim = 2 # This changes according to the measurement
kf.P = np.eye(M) * 0.3
kf.x = state.reshape(-1).copy() # Initial state guess
z_var = 0.05**2
kf.R = z_var # Observation variance
dpos_var = 0.01**2 * dt
drot_var = np.radians(1.0)**2 * dt
#kf.Q = np.diag([0]*3 + [0]*3 + [dpos_var]*3 + [drot_var]*3)
kf.Q = np.diag([dpos_var] * 3 + [drot_var] * 3)
all_world_positions = []
for w in world_markers.values():
for v in w:
all_world_positions.append(v)
all_world_positions = np.array(all_world_positions)
for row in markerss:
markers = row['markers']
world_positions = []
screen_positions = []
for m in markers:
try:
world = world_markers[str(m['id'])]
except KeyError:
continue
if m['id_confidence'] < 0.7: continue
for w, s in zip(world, m['verts']):
world_positions.append(w)
screen_positions.append(s)
world_positions = np.array(world_positions).reshape(-1, 2)
screen_positions = np.array(screen_positions).reshape(-1, 2)
screen_positions[:, 0] -= c_x
screen_positions[:, 0] /= f_x
screen_positions[:, 1] -= c_y
screen_positions[:, 1] /= -f_y
kf.predict()
if len(world_positions) >= 0:
measurement = screen_positions.reshape(-1)
kf.update(measurement,
R=np.diag([z_var] * len(measurement)),
world_positions=world_positions)
yield kf.x.copy(), kf.P.copy()
def test_circle():
from filterpy.kalman import KalmanFilter
from math import radians
def hx(x):
radius = x[0]
angle = x[1]
x = cos(radians(angle)) * radius
y = sin(radians(angle)) * radius
return np.array([x, y])
def fx(x, dt):
return np.array([x[0], x[1] + x[2], x[2]])
std_noise = .1
sp = JulierSigmaPoints(n=3, kappa=0.)
f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, points=sp)
f.x = np.array([50., 90., 0])
f.P *= 100
f.R = np.eye(2) * (std_noise**2)
f.Q = np.eye(3) * .001
f.Q[0, 0] = 0
f.Q[2, 2] = 0
kf = KalmanFilter(dim_x=6, dim_z=2)
kf.x = np.array([50., 0., 0, 0, .0, 0.])
F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5],
[0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]])
kf.F = F
kf.P *= 100
kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
kf.R = f.R
kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001)
kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001)
measurements = []
results = []
zs = []
kfxs = []
for t in range(0, 12000):
a = t / 30 + 90
x = cos(radians(a)) * 50. + randn() * std_noise
y = sin(radians(a)) * 50. + randn() * std_noise
# create measurement = t plus white noise
z = np.array([x, y])
zs.append(z)
f.predict()
f.update(z)
kf.predict()
kf.update(z)
# save data
results.append(hx(f.x))
kfxs.append(kf.x)
#print(f.x)
results = np.asarray(results)
zs = np.asarray(zs)
kfxs = np.asarray(kfxs)
print(results)
if DO_PLOT:
plt.plot(zs[:, 0], zs[:, 1], c='r', label='z')
plt.plot(results[:, 0], results[:, 1], c='k', label='UKF')
plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF')
plt.legend(loc='best')
plt.axis('equal')
####Net.Control_Input(Directory+'5_pipes_driving_transient.csv') ####Net.MOC_Run(maxTime) ##pp.show() from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints, JulierSigmaPoints, JulierSigmaPoints import scipy.stats as stats #InitP = np.load(Directory+'UKFInitP.npy') #P[:2*Net.CPs,:2*Net.CPs] = InitP[:2*Net.CPs,:2*Net.CPs] ukf_mean = State ukf_cov = nearestPD(P) Iterations = 3000 points = MerweScaledSigmaPoints(n=State.size, alpha=1e-3, beta=2., kappa=3-State.size) points = JulierSigmaPoints(n=State.size,kappa = 0) sigmas = points.sigma_points(ukf_mean,ukf_cov) sigmas_f = np.empty((Iterations,sigmas.shape[0],sigmas.shape[1])) sigmas_f[0,:,:] = sigmas for i in range(sigmas.shape[0]): print i #print s ret,reservoir = epa.ENgetnodeindex(str(Net.nodes[-1].Name)) ret,demand = epa.ENgetnodeindex(str(Net.nodes[1].Name)) Net.nodes[1].demand = sigmas_f[0,i,-1] ret,downstream = epa.ENgetnodeindex(str(Net.nodes[-2].Name)) Net.nodes[-2].demand = sigmas_f[0,i,-2] #demand_generator(Directory+'5_pipes_driving_transient.csv',6,maxTime,dt,sigmas_f[0,i,-2]*1000.,sigmas_f[0,i,-2]*1000.+0.5,2,30) #Net.Control_Input(Directory+'5_pipes_driving_transient.csv')
def __init__(self,
dim,
limit,
dim_z=2,
fx=None,
W=None,
obs_noise_func=None,
collision_func=None,
sampling_period=0.5,
kappa=1):
"""
dim : dimension of state
***Assuming dim==3: (x,y,theta), dim==4: (x,y,xdot,ydot), dim==5: (x,y,theta,v,w)
limit : An array of two vectors. limit[0] = minimum values for the state,
limit[1] = maximum value for the state
dim_z : dimension of observation,
fx : x_tp1 = fx(x_t, dt), state dynamic function
W : state noise matrix
obs_noise_func : observation noise matrix function of z
collision_func : collision checking function
n : the number of sigma points
"""
self.dim = dim
self.limit = limit
self.W = W if W is not None else np.zeros((self.dim, self.dim))
self.obs_noise_func = obs_noise_func
self.collision_func = collision_func
def hx(y, agent_state, measure_func=util.relative_distance_polar):
r_pred, alpha_pred = measure_func(y[:2], agent_state[:2],
agent_state[2])
return np.array([r_pred, alpha_pred])
def x_mean_fn_(sigmas, Wm):
if dim == 3:
x = np.zeros(dim)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
x[1] += s[1] * Wm[i]
sum_sin += np.sin(s[2]) * Wm[i]
sum_cos += np.cos(s[2]) * Wm[i]
x[2] = np.arctan2(sum_sin, sum_cos)
return x
elif dim == 5:
x = np.zeros(dim)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
x[1] += s[1] * Wm[i]
x[3] += s[3] * Wm[i]
x[4] += s[4] * Wm[i]
sum_sin += np.sin(s[2]) * Wm[i]
sum_cos += np.cos(s[2]) * Wm[i]
x[2] = np.arctan2(sum_sin, sum_cos)
return x
else:
return None
def z_mean_fn_(sigmas, Wm):
x = np.zeros(dim_z)
sum_sin, sum_cos = 0., 0.
for i in range(len(sigmas)):
s = sigmas[i]
x[0] += s[0] * Wm[i]
sum_sin += np.sin(s[1]) * Wm[i]
sum_cos += np.cos(s[1]) * Wm[i]
x[1] = np.arctan2(sum_sin, sum_cos)
return x
def residual_x_(x, xp):
"""
x : state, [x, y, theta]
xp : predicted state
"""
if dim == 3 or dim == 5:
r_x = x - xp
r_x[2] = util.wrap_around(r_x[2])
return r_x
else:
return None
def residual_z_(z, zp):
"""
z : observation, [r, alpha]
zp : predicted observation
"""
r_z = z - zp
r_z[1] = util.wrap_around(r_z[1])
return r_z
sigmas = JulierSigmaPoints(n=dim, kappa=kappa)
self.ukf = UnscentedKalmanFilter(dim,
dim_z,
sampling_period,
fx=fx,
hx=hx,
points=sigmas,
x_mean_fn=x_mean_fn_,
z_mean_fn=z_mean_fn_,
residual_x=residual_x_,
residual_z=residual_z_)
def Attitude_Filter(lightcurve,
obsvecs,
sunvecs,
time,
noise,
Geometry,
Inertia_Matrix,
exposure_time,
est_inertia=False):
dts = diff(time)
ave_dt = average(dts)
if est_inertia:
DIM_X = 8
DIM_Z = 1
points = JulierSigmaPoints(DIM_X)
initial_rate = array([.01, .01, .01])
initial_mrps = array([.01, .01, .01])
initial_inertia = array([1.0, 1.0])
initial_state = hstack([initial_mrps, initial_rate, initial_inertia])
kf = UnscentedKalmanFilter(dim_x=DIM_X,
dim_z=DIM_Z,
dt=ave_dt,
fx=modified_rodrigues_prop,
hx=mrp_measurement_function,
points=points)
kf.x = initial_state
kf.P = diag(ones(DIM_X)) * 100
kf.R = noise**2
kf.Q = diag(
[1, 1, 1, 1e3, 1e3, 1e3, 1e3, 1e3]
) * 1e-12 #I suspect that the last two values here are not tuned super great. Those in particular correspond to the model uncertainty of the inertia values
else:
DIM_X = 6
DIM_Z = 1
points = JulierSigmaPoints(DIM_X)
initial_rate = array([.01, .01, .01])
initial_mrps = array([.01, .01, .01])
initial_state = hstack([initial_mrps, initial_rate])
kf = UnscentedKalmanFilter(dim_x=DIM_X,
dim_z=DIM_Z,
dt=ave_dt,
fx=modified_rodrigues_prop,
hx=mrp_measurement_function,
points=points)
kf.x = initial_state
kf.P = diag(ones(DIM_X)) * 100
kf.R = noise**2
kf.Q = diag([1, 1, 1, 1e3, 1e3, 1e3]) * 1e-12
#print(kf.Q)
# plt.plot(noisy_lightcurve)
# plt.plot(lightcurve)
# plt.show()
means = zeros((len(lightcurve), DIM_X))
covariances = zeros((len(lightcurve), DIM_X, DIM_X))
residuals = zeros(len(lightcurve))
filtered_lightcurve = zeros(len(lightcurve))
for i, (z, obsvec, sunvec,
dt) in enumerate(zip(lightcurve, obsvecs, sunvecs, dts)):
kf.update(z,
obsvec=obsvec,
sunvec=sunvec,
exposure_time=exposure_time,
Geometry=Geometry)
kf.predict(dt=dt, inertia=Inertia_Matrix, est_inertia=est_inertia)
#print('[{0:+.4f}, {1:+.4f}, {2:+.4f}] [{3:+.4f}, {4:+.4f}, {5:+.4f}] {6:<10}'.format(*kf.x, i), end = '\r')
#switch from mrps to shadow parameters
if norm(kf.x[0:3]) > 1:
mrps = kf.x[0:3]
kf.x[0:3] = -mrps / norm(mrps)**2
#keep angular velocity from exploding
#limits maximum estimate to be 1 radian/s
if norm(kf.x[3:6]) > 1:
kf.x[3:6] = kf.x[3:6] / norm(kf.x[3:6])
#keep inertia from exploding
#limits maximum inertia estimate to be 5
if est_inertia:
if abs(kf.x[6]) > 5:
kf.x[6] = 5
if abs(kf.x[7]) > 5:
kf.x[7] = 5
#print(kf.sigmas_f)
#print(kf.y)
loading_bar(i / len(lightcurve), 'Filtering Data')
means[i, :] = kf.x
covariances[i, :, :] = kf.P
residuals[i] = kf.y
filtered_lightcurve[i] = kf.zp
return means, covariances, residuals, filtered_lightcurve
obstacle_threshold = 15
voltage_threshold = 6.7
uv_counter = 0
orientation = rpi_orientation()
yaw_gyro,yaw_mag,yaw_mag_compensated =0,0,0
ts, yaws_gyro, yaws_mag, yaws_mag_compensated = [], [], [], []
rolls, pitchs = [], []
sigmas = JulierSigmaPoints(n=2, kappa=1)
def fx(x, dt):
xout = np.empty_like(x)
xout[0] = x[1] * dt + x[0]
xout[1] = x[1]
return xout
def hx(x):
return x[:1] # return position [x]
ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=1., hx=hx, fx=fx, points=sigmas)
ukf.P *= 10
ukf.R *= .5
ukf.Q = Q_discrete_white_noise(2, dt=1., var=0.03)
while(1):
def test_fixed_lag():
def fx(x, dt):
A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
f = np.dot(A, x)
return f
def hx(x):
return np.sqrt(x[0]**2 + x[2]**2)
dt = 0.05
sp = JulierSigmaPoints(n=3, kappa=0)
kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp)
kf.Q *= 0.01
kf.R = 10
kf.x = np.array([0., 90., 1100.])
kf.P *= 1.
radar = RadarSim(dt)
t = np.arange(0, 20 + dt, dt)
n = len(t)
xs = np.zeros((n, 3))
random.seed(200)
rs = []
#xs = []
M = []
P = []
N = 10
flxs = []
for i in range(len(t)):
r = radar.get_range()
#r = GetRadar(dt)
kf.predict()
kf.update(z=[r])
xs[i, :] = kf.x
flxs.append(kf.x)
rs.append(r)
M.append(kf.x)
P.append(kf.P)
print(i)
#print(i, np.asarray(flxs)[:,0])
if i == 20 and len(M) >= N:
try:
M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:],
Ps=np.asarray(P)[-N:])
flxs[-N:] = M2
#flxs[-N:] = [20]*N
except:
print('except', i)
#P[-N:] = P2
kf.x = np.array([0., 90., 1100.])
kf.P = np.eye(3) * 100
M, P = kf.batch_filter(rs)
Qs = [kf.Q] * len(t)
M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs)
flxs = np.asarray(flxs)
print(xs[:, 0].shape)
plt.figure()
plt.subplot(311)
plt.plot(t, xs[:, 0])
plt.plot(t, flxs[:, 0], c='r')
plt.plot(t, M2[:, 0], c='g')
plt.subplot(312)
plt.plot(t, xs[:, 1])
plt.plot(t, flxs[:, 1], c='r')
plt.plot(t, M2[:, 1], c='g')
plt.subplot(313)
plt.plot(t, xs[:, 2])
plt.plot(t, flxs[:, 2], c='r')
plt.plot(t, M2[:, 2], c='g')
Ns = 12
ukf_test = ukf_uav.UnscentedKalmanFilter(Ns, Ns, 0.01)
q = 1
r = 1
ukf_test.J = J
ukf_test.e3 = e3
Q = q**2*np.eye(Ns)
R = r**2
x = np.zeros(Ns)
P = np.eye(Ns)
x_ukf = []
x_sensor = []
sp = JulierSigmaPoints(Ns,5)
def state_tran(x, dt):
A = np.eye(12)
for i in range(3):
A[i,i+3] = dt
A[i+6,i+9] = dt
return np.dot(A,x)
def obs_state(x):
A = np.zeros((6,12))
for i in range(3):
A[i,i+3] = 1
A[i+3, i + 9] = 1
return np.dot(A,x)
ukf_filter = UKF.UnscentedKalmanFilter(dim_x=Ns,dim_z=6, dt=0.01, hx = obs_state, fx = state_tran, points = sp)
ukf_filter.Q = Q
ukf_filter.R = R
