def plot_sigma_points():
x = np.array([0, 0])
P = np.array([[4, 2], [2, 4]])
sigmas = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=1.)
S0 = sigmas.sigma_points(x, P)
Wm0, Wc0 = sigmas.Wm, sigmas.Wc
sigmas = MerweScaledSigmaPoints(n=2, alpha=1., beta=2., kappa=1.)
S1 = sigmas.sigma_points(x, P)
Wm1, Wc1 = sigmas.Wm, sigmas.Wc
def plot_sigmas(s, w, **kwargs):
min_w = min(abs(w))
scale_factor = 100 / min_w
return plt.scatter(s[:, 0], s[:, 1], s=abs(w)*scale_factor, alpha=.5, **kwargs)
plt.subplot(121)
plot_sigmas(S0, Wc0, c='b')
plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4])
plt.title('alpha=0.3')
plt.subplot(122)
plot_sigmas(S1, Wc1, c='b', label='Kappa=2')
plot_covariance_ellipse(x, P, facecolor='g', alpha=0.2, variance=[1, 4])
plt.title('alpha=1')
plt.show()
def show_sigma_transform(with_text=False):
fig = plt.figure()
ax=fig.gca()
x = np.array([0, 5])
P = np.array([[4, -2.2], [-2.2, 3]])
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=9)
sigmas = MerweScaledSigmaPoints(2, alpha=.5, beta=2., kappa=0.)
S = sigmas.sigma_points(x=x, P=P)
plt.scatter(S[:,0], S[:,1], c='k', s=80)
x = np.array([15, 5])
P = np.array([[3, 1.2],[1.2, 6]])
plot_covariance_ellipse(x, P, facecolor='g', variance=9, alpha=0.3)
ax.add_artist(arrow(S[0,0], S[0,1], 11, 4.1, 0.6))
ax.add_artist(arrow(S[1,0], S[1,1], 13, 7.7, 0.6))
ax.add_artist(arrow(S[2,0], S[2,1], 16.3, 0.93, 0.6))
ax.add_artist(arrow(S[3,0], S[3,1], 16.7, 10.8, 0.6))
ax.add_artist(arrow(S[4,0], S[4,1], 17.7, 5.6, 0.6))
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
if with_text:
plt.text(2.5, 1.5, r"$\chi$", fontsize=32)
plt.text(13, -1, r"$\mathcal{Y}$", fontsize=32)
#plt.axis('equal')
plt.show()
def print_sigmas(n=1, mean=5, cov=3, alpha=.1, beta=2., kappa=2):
points = MerweScaledSigmaPoints(n, alpha, beta, kappa)
print('sigmas: ', points.sigma_points(mean, cov).T[0])
Wm, Wc = points.weights()
print('mean weights:', Wm)
print('cov weights:', Wc)
print('lambda:', alpha**2 *(n+kappa) - n)
print('sum cov', sum(Wc))
def test_scaled_weights():
for n in range(1,5):
for alpha in np.linspace(0.99, 1.01, 100):
for beta in range(0,2):
for kappa in range(0,2):
sp = MerweScaledSigmaPoints(n, alpha, 0, 3-n)
Wm, Wc = sp.weights()
assert abs(sum(Wm) - 1) < 1.e-1
assert abs(sum(Wc) - 1) < 1.e-1
def show_sigma_selections():
ax=plt.gca()
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
x = np.array([2, 5])
P = np.array([[3, 1.1], [1.1, 4]])
points = MerweScaledSigmaPoints(2, .09, 2., 1.)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
x = np.array([5, 5])
points = MerweScaledSigmaPoints(2, .15, 1., .15)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
x = np.array([8, 5])
points = MerweScaledSigmaPoints(2, .2, 3., 10)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
plt.axis('equal')
plt.xlim(0,10); plt.ylim(0,10)
plt.show()
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 plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
def fx(x):
return x**3
def dfx(x):
return 3*x**2
mean = 1
var = .1
std = math.sqrt(var)
data = normal(loc=mean, scale=std, size=50000)
d_t = fx(data)
points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
Wm, Wc = points.Wm, points.Wc
sigmas = points.sigma_points(mean, var)
sigmas_f = np.zeros((3, 1))
for i in range(3):
sigmas_f[i] = fx(sigmas[i, 0])
### pass through unscented transform
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
ukf_mean = ukf_mean[0]
ukf_std = math.sqrt(ukf_cov[0])
norm = scipy.stats.norm(ukf_mean, ukf_std)
xs = np.linspace(-3, 5, 200)
plt.plot(xs, norm.pdf(xs), ls='--', lw=2, color='b')
try:
plt.hist(d_t, bins=200, density=True, histtype='step', lw=2, color='g')
except:
# older versions of matplotlib don't have the density keyword
plt.hist(d_t, bins=200, normed=True, histtype='step', lw=2, color='g')
actual_mean = d_t.mean()
plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo')
plt.axvline(ukf_mean, lw=2, ls='--', color='b', label='UKF')
plt.legend()
plt.show()
print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std()))
print('UKF mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
def show_sigma_selections():
ax=plt.gca()
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
x = np.array([2, 5])
P = np.array([[3, 1.1], [1.1, 4]])
points = MerweScaledSigmaPoints(2, .05, 2., 1.)
sigmas = points.sigma_points(x, P)
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5])
plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50)
x = np.array([5, 5])
points = MerweScaledSigmaPoints(2, .15, 2., 1.)
sigmas = points.sigma_points(x, P)
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5])
plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50)
x = np.array([8, 5])
points = MerweScaledSigmaPoints(2, .4, 2., 1.)
sigmas = points.sigma_points(x, P)
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.6, variance=[.5])
plt.scatter(sigmas[:,0], sigmas[:, 1], c='k', s=50)
plt.axis('equal')
plt.xlim(0,10); plt.ylim(0,10)
plt.show()
def plot_ukf_vs_mc(alpha=0.001, beta=3.0, kappa=1.0):
def fx(x):
return x ** 3
def dfx(x):
return 3 * x ** 2
mean = 1
var = 0.1
std = math.sqrt(var)
data = normal(loc=mean, scale=std, size=50000)
d_t = fx(data)
points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, var)
sigmas_f = np.zeros((3, 1))
for i in range(3):
sigmas_f[i] = fx(sigmas[i, 0])
### pass through unscented transform
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
ukf_mean = ukf_mean[0]
ukf_std = math.sqrt(ukf_cov[0])
norm = scipy.stats.norm(ukf_mean, ukf_std)
xs = np.linspace(-3, 5, 200)
plt.plot(xs, norm.pdf(xs), ls="--", lw=2, color="b")
plt.hist(d_t, bins=200, normed=True, histtype="step", lw=2, color="g")
actual_mean = d_t.mean()
plt.axvline(actual_mean, lw=2, color="g", label="Monte Carlo")
plt.axvline(ukf_mean, lw=2, ls="--", color="b", label="UKF")
plt.legend()
plt.show()
print("actual mean={:.2f}, std={:.2f}".format(d_t.mean(), d_t.std()))
print("UKF mean={:.2f}, std={:.2f}".format(ukf_mean, ukf_std))
def test_linear_2d_merwe():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array(
[[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]],
dtype=float)
return np.dot(F, x)
def hx(x):
return np.array([x[0], x[2]])
dt = 0.1
points = MerweScaledSigmaPoints(4, .1, 2., -1)
kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1])
kf.P *= 0.0001
# test __repr__ doesn't crash
str(kf)
zs = []
for i in range(20):
z = np.array([i + randn() * 0.1, i + randn() * 0.1])
zs.append(z)
Ms, Ps = kf.batch_filter(zs)
smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt)
if DO_PLOT:
plt.figure()
zs = np.asarray(zs)
plt.plot(zs[:, 0], marker='+')
plt.plot(Ms[:, 0], c='b')
plt.plot(smooth_x[:, 0], smooth_x[:, 2], c='r')
print(smooth_x)
def test_linear_1d():
""" should work like a linear KF if problem is linear """
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([x[0]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UnscentedKalmanFilter(dim_x=2,
dim_z=1,
dt=dt,
fx=fx,
hx=hx,
points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
z = np.array([2.])
kf.predict()
kf.update(z)
zs = []
for i in range(50):
z = np.array([i + randn() * 0.1])
zs.append(z)
kf.predict()
kf.update(z)
print('K', kf.K.T)
print('x', kf.x)
def estimateUKF(camPoses):
points = MerweScaledSigmaPoints(3, .1, 2., 0.)
filter = UKF(3,
3,
0,
h,
f,
points,
sqrt_fn=None,
x_mean_fn=state_mean,
z_mean_fn=state_mean,
residual_x=residual,
residual_z=residual)
filter.P = np.diag([0.04, 0.04, 0.003])
filter.Q = np.diag([(0.2)**2, (0.2)**2, (1 * 3.14 / 180)**2])
filter.R = np.diag([100, 100, 0.25])
Uposes = [[], []]
for i in range(len(speed)):
x = filter.x
Uposes[0].append(x[0])
Uposes[1].append(x[1])
filter.predict(fx_args=[speed[i], angle[i] * 0.01745])
filter.update(z=[camPoses[0][i], camPoses[1][i], camPoses[2][i]])
return Uposes
def test_1d():
def fx(x, dt):
F = np.array([[1., dt], [0, 1]])
return np.dot(F, x)
def hx(x):
return np.array([[x[0]]])
ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx)
ckf.x = np.array([[1.], [2.]])
ckf.P = np.array([[1, 1.1], [1.1, 3]])
ckf.R = np.eye(1) * .05
ckf.Q = np.array([[0., 0], [0., .001]])
dt = 0.1
points = MerweScaledSigmaPoints(2, .1, 2., -1)
kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([1, 2])
kf.P = np.array([[1, 1.1], [1.1, 3]])
kf.R *= 0.05
kf.Q = np.array([[0., 0], [0., .001]])
for i in range(50):
z = np.array([[i + randn() * 0.1]])
#xx, pp, Sx = predict(f, x, P, Q)
#x, P = update(h, z, xx, pp, R)
ckf.predict()
ckf.update(z)
kf.predict()
kf.update(z[0])
assert abs(ckf.x[0] - kf.x[0]) < 1e-10
assert abs(ckf.x[1] - kf.x[1]) < 1e-10
def __init__(self, dt, wheel_base, var_rate_enc, meas_da):
sp = MerweScaledSigmaPoints(n=8, alpha=0.3, beta=2., kappa=-1.)
UnscentedKalmanFilter.__init__(
self,
dim_x=8,
dim_z=4,
dt=dt,
fx=TankKalmanDelta3.f_func,
residual_x=TankKalmanDelta3.x_residual_fn,
x_mean_fn=TankKalmanDelta3.x_mean_fn,
hx=TankKalmanDelta3.h_func,
residual_z=TankKalmanDelta3.h_residual_fn,
z_mean_fn=TankKalmanDelta3.h_mean_fn,
points=sp)
self.wheel_base = wheel_base
self.var_a_s = 4.0
self.var_a_p = 1e-10
self.var_a_a = math.radians(45.0)
# starting point and covariance
self.x = np.array(8 * [
0.,
])
sig_a_start = math.radians(5.0)
self.P = np.diag([
0.5**2, 0.5**2, dt**2 * self.var_a_s, self.var_a_s,
dt**2 * self.var_a_p, sig_a_start**2, dt**2 * self.var_a_a,
self.var_a_a
])
self.meas_da = meas_da
self.prev_x = None
self.prev_encoders = [0, 0, 0]
self.tank_encoder = TankEncoderTracker(wheel_base, var_rate_enc,
var_rate_enc)
return
def create_ukf(vehicle, landmarks, P0, Q, dt, extents):
def f(x, dt, u=np.zeros(2), extents=extents):
vehicle.x = x.copy()
print(u, dt)
vehicle.move(u=u, dt=dt, extents=extents)
return vehicle.x
def h(x, landmark=None):
assert landmark is not None
hx = np.empty(2)
lx, ly = landmark
hx[0] = np.sqrt((lx - x[0])**2 + (ly - x[1])**2)
hx[1] = np.arctan2(ly - x[1], lx - x[0])
return hx
def residual_h(z, h):
"""Subtract [range, bearing] vectors, accounting for angle"""
y = z - h
y[1] = mpi_to_pi(y[1])
wrap(y, extents)
return y
def residual_x(xa, xb):
"""Subtract [x, y, phi] vectors, accounting for angle"""
dx = xa - xb
dx[2] = mpi_to_pi(dx[2])
wrap(dx, extents)
return dx
def state_mean(sigmas, w_m):
x = np.empty(3)
# This is a hack (and not a very good one) to handle periodic boundary
# crossings. Seems to help a little.
std = np.std(sigmas[:, :2])
if std > (extents[1] - extents[0]) / 4:
x[:2] = sigmas[0, :2]
else:
x[0] = np.dot(sigmas[:, 0], w_m)
x[1] = np.dot(sigmas[:, 1], w_m)
sum_sin = np.dot(np.sin(sigmas[:, 2]), w_m)
sum_cos = np.dot(np.cos(sigmas[:, 2]), w_m)
x[2] = np.arctan2(sum_sin, sum_cos)
wrap(x, extents)
return x
def z_mean(sigmas, w_m):
z_count = sigmas.shape[1]
x = np.zeros(z_count)
for z in range(0, z_count, 2):
sum_sin = np.dot(np.sin(sigmas[:, z + 1]), w_m)
sum_cos = np.dot(np.cos(sigmas[:, z + 1]), w_m)
x[z] = np.dot(sigmas[:, z], w_m)
x[z + 1] = np.arctan2(sum_sin, sum_cos)
return x
points = MerweScaledSigmaPoints(n=3,
alpha=1e-3,
beta=2,
kappa=0,
subtract=residual_x)
ukf = UKF(
dim_x=3,
dim_z=2,
fx=f,
hx=h,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h,
)
ukf.x = vehicle.x.copy()
ukf.P = P0.copy()
ukf.Q = Q
# This large scale factor is theoretically unjustified.
# TODO: without it, the sim crashes. Figure this out!
ukf.R = 1e5 * np.diag([vehicle.range_sigma**2, vehicle.bearing_sigma**2])
# ukf.R = np.diag([vehicle.range_sigma**2, vehicle.bearing_sigma**2])
return ukf
# from scipy.spatial import Voronoi, voronoi_plot_2d
# import skfmm
logger = logging.getLogger("vision")
logger.addHandler(logging.StreamHandler())
logger.setLevel(logging.DEBUG)
# import vision
# import datetime
# from scipy import interpolate
# from vision import controllers
# import filterpy
from filterpy.kalman import MerweScaledSigmaPoints
# from filterpy.kalman import UnscentedKalmanFilter as UKF
dt = 0.1
sigmas = MerweScaledSigmaPoints(5, alpha=.1, beta=2., kappa=1.)
from planebots.control import dwa
import time
# import vision.model as model
def tick2thomega(ticks, dt):
thomega = np.array(
[0.5 * (ticks[0] + ticks[1]),
(ticks[0] - ticks[1]) / 0.0408]) / 170 # wheelbase of 40.8mm
return thomega
class TestDwa(unittest.TestCase):
def fit(sysargv):
"""
Program to process Covid Data.
:Example:
For countries (European database)
>> python ProcessSEIR1R2D.py
>> python ProcessSEIR1R2D.py France 0 1 0 0 1 1
>> python ProcessSEIR1R2D.py France 2 3 8 0 1 1 # 3 périodes pour les femmes en France avec un décalage de 8 jours
>> python ProcessSEIR1R2D.py France,Germany 1 1 0 0 1 1 # 1 période pour les hommes francais et les hommes allemands
For French Region (French database)
>> python ProcessSEIR1R2D.py FRANCE,D69 0 -1 13 0 1 1 # Code Insee Dpt 69 (Rhône)
>> python ProcessSEIR1R2D.py FRANCE,R84 0 -1 13 0 1 1 # Tous les dpts de la Région dont le code Insee est
>> python ProcessSEIR1R2D.py FRANCE,R32+ 0 -1 13 0 1 1 # Somme de tous les dpts de la Région 32 (Hauts-de-France)
>> python ProcessSEIR1R2D.py FRANCE,MetropoleD 0 -1 13 0 1 1 # Tous les départements de la France métropolitaine
>> python ProcessSEIR1R2D.py FRANCE,MetropoleD+ 0 -1 13 0 1 1 # Toute la France métropolitaine (en sommant les dpts)
>> python ProcessSEIR1R2D.py FRANCE,MetropoleR+ 0 -1 13 0 1 1 # Somme des dpts de toutes les régions françaises
Toute combinaison est possible de lieu : exemple FRANCE,R32+,D05,R84
argv[1] : List of countries (ex. France,Germany,Italy), or see above. Default: France
argv[2] : Sex (male:1, female:2, male+female:0). Only for french database Default: 0
argv[3] : Periods ('1' -> 1 period ('all-in-on'), '!=1' -> severall periods). Default: -1
argv[4] : Delay (in days). Default: 13
argv[5] : UKF filtering of data (0/1). Default: 0
argv[6] : Verbose level (debug: 3, ..., almost mute: 0). Default: 1
argv[7] : Plot graphique (0/1). Default: 1
"""
#Austria,Belgium,Croatia,Czechia,Finland,France,Germany,Greece,Hungary,Ireland,Italy,Lithuania,Poland,Portugal,Romania,Serbia,Spain,Switzerland,Ukraine
#Austria,Belgium,Croatia,Czechia,Finland,France,Germany,Greece,Hungary,Ireland,Italy,Poland,Portugal,Romania,Serbia,Spain,Switzerland,Ukraine
# Il y a 18 pays
if len(sysargv) > 7:
print(' CAUTION : bad number of arguments - see help')
exit(1)
# Constantes
######################################################@
fileLocalCopy = True # if we upload the file from the url (to get latest data) or from a local copy file
readStartDateStr = "2020-03-01" # "2020-03-01" Le 8 mars, pour inclure un grand nombre de pays européens dont la date de premier était postérieur au 1er mars
readStopDateStr = None
recouvrement = -1
dt = 1
France = 'France'
thresholdSignif = 1.5E-6
# Interpetation of arguments - reparation
######################################################@
# Default value for parameters
listplaces = ['France']
sexe, sexestr = 0, 'male+female'
nbperiodes = -1
decalage = 13
UKF_filt = False
verbose = 1
plot = True
# Parameters from argv
if len(sysargv) > 0: liste = list(sysargv[0].split(','))
if len(sysargv) > 1: sexe = int(sysargv[1])
if len(sysargv) > 2: nbperiodes = int(sysargv[2])
if len(sysargv) > 3: decalage = int(sysargv[3])
if len(sysargv) > 4 and int(sysargv[4]) == 1: UKF_filt = True
if len(sysargv) > 5: verbose = int(sysargv[5])
if len(sysargv) > 6 and int(sysargv[6]) == 0: plot = False
if nbperiodes == 1:
decalage = 0 # nécessairement pas de décalage (on compense le recouvrement)
if sexe not in [0, 1, 2]:
sexe, sexestr = 0, 'male+female' # sexe indiférencié
if sexe == 1: sexestr = 'male'
if sexe == 2: sexestr = 'female'
listplaces = []
listnames = []
if liste[0] == 'FRANCE':
FrDatabase = True
liste = liste[1:]
for el in liste:
l, n = getPlace(el)
if el == 'MetropoleR+':
for l1, n1 in zip(l, n):
listplaces.extend(l1)
listnames.extend([n1])
else:
listplaces.extend(l)
listnames.extend(n)
else:
listplaces = liste[:]
FrDatabase = False
if verbose > 0:
print(' Full command line : ' + sysargv[0] + ' ' + str(nbperiodes) +
' ' + str(decalage) + ' ' + str(UKF_filt) + ' ' + str(verbose) +
' ' + str(plot),
flush=True)
# Data reading to get first and last date available in the data set
######################################################@
if FrDatabase == True:
pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, _ = readDataFrance(
['D69'],
readStartDateStr,
readStopDateStr,
fileLocalCopy,
sexe,
verbose=0)
else:
pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, _ = readDataEurope(
France,
readStartDateStr,
readStopDateStr,
fileLocalCopy,
verbose=0)
dataLength = pd_exerpt.shape[0]
readStartDate = datetime.strptime(readStartDateStr, strDate)
if readStartDate < pd_exerpt.index[0]:
readStartDate = pd_exerpt.index[0]
readStartDateStr = pd_exerpt.index[0].strftime(strDate)
readStopDate = datetime.strptime(readStopDateStr, strDate)
if readStopDate < pd_exerpt.index[-1]:
readStopDate = pd_exerpt.index[-1]
readStopDateStr = pd_exerpt.index[-1].strftime(strDate)
dataLength = pd_exerpt.shape[0]
if verbose > 1:
print('readStartDateStr=', readStartDateStr, ', readStopDateStr=',
readStopDateStr)
print('readStartDate =', readStartDate, ', readStopDate =',
readStopDate)
print('dataLength =', dataLength)
#input('pause')
# Collections of data return by this function
modelSEIR1R2D = np.zeros(shape=(len(listplaces), dataLength, 6))
data_deriv = np.zeros(shape=(len(listplaces), dataLength, 2))
modelR1_deriv = np.zeros(shape=(len(listplaces), dataLength, 2))
data_all = np.zeros(shape=(len(listplaces), dataLength, 2))
modelR1_all = np.zeros(shape=(len(listplaces), dataLength, 2))
Listepd = []
ListetabParamModel = []
# data observed
data = np.zeros(shape=(dataLength, 2))
# Paramètres sous forme de chaines de caractères
ListeTextParam = []
ListeDateI0 = []
# Loop on the places to process
for indexplace, place in enumerate(listplaces):
# Get the full name of the place to process, and the special dates corresponding to the place
if FrDatabase == True:
placefull = 'France-' + listnames[indexplace][0]
DatesString = readDates(France, verbose)
else:
placefull = place
DatesString = readDates(place, verbose)
print('PROCESSING of', placefull, 'in', listnames)
# data reading of the observations
#############################################################################
if FrDatabase == True:
pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, dateFirstNonZeroStr = readDataFrance(
place,
readStartDateStr,
readStopDateStr,
fileLocalCopy,
sexe,
verbose=0)
else:
pd_exerpt, HeadData, N, readStartDateStr, readStopDateStr, dateFirstNonZeroStr = readDataEurope(
place,
readStartDateStr,
readStopDateStr,
fileLocalCopy,
verbose=0)
shift_0value = getNbDaysBetweenDateFromString(readStartDateStr,
dateFirstNonZeroStr)
# UKF Filtering ?
if UKF_filt == True:
data2Filt = pd_exerpt[[HeadData[0],
HeadData[1]]].to_numpy(copy=True)
sigmas = MerweScaledSigmaPoints(n=2, alpha=.5, beta=2.,
kappa=1.) #1-3.)
ukf = UKF(dim_x=2, dim_z=2, fx=fR1D, hx=hR1D, dt=dt, points=sigmas)
# Filter init
ukf.x[:] = data2Filt[0, :]
ukf.Q = np.diag([30., 15.])
ukf.R = np.diag([170., 100.])
if verbose > 1:
print('ukf.x[:]=', ukf.x[:])
print('ukf.R =', ukf.R)
print('ukf.Q =', ukf.Q)
# UKF filtering and smoothing, batch mode
R1Ffilt, _ = ukf.batch_filter(data2Filt)
HeadData[0] += ' filt'
HeadData[1] += ' filt'
pd_exerpt[HeadData[0]] = R1Ffilt[:, 0]
pd_exerpt[HeadData[1]] = R1Ffilt[:, 1]
# Get the list of dates to process
ListDates, ListDatesStr = GetPairListDates(readStartDate, readStopDate,
DatesString, decalage,
nbperiodes, recouvrement)
if verbose > 1:
#print('ListDates =', ListDates)
print('ListDatesStr=', ListDatesStr)
#input('pause')
# Solveur edo
solveur = SolveEDO_SEIR1R2D(N, dt, verbose)
indexdata = solveur.indexdata
E0, I0, R10, R20, D0 = 0, 1, 0, 0, 0
# Repertoire des figures
if plot == True:
repertoire = getRepertoire(
UKF_filt, './figures/SEIR1R2D_UKFilt/' + placefull + '/sexe_' +
str(sexe) + '_delay_' + str(decalage), './figures/SEIR1R2D/' +
placefull + '/sexe_' + str(sexe) + '_delay_' + str(decalage))
prefFig = repertoire + '/Process_'
# Remise à 0 des données
data.fill(0.)
# Boucle pour traiter successivement les différentes fenêtres
###############################################################
ListeTextParamPlace = []
ListetabParamModelPlace = []
ListeEQM = []
DEGENERATE_CASE = False
for i in range(len(ListDatesStr)):
# dates of the current period
fitStartDate, fitStopDate = ListDates[i]
fitStartDateStr, fitStopDateStr = ListDatesStr[i]
# Est-on dans un CAS degénéré?
# print(getNbDaysBetweenDateFromString(dateFirstNonZeroStr, fitStopDateStr))
if getNbDaysBetweenDateFromString(
dateFirstNonZeroStr, fitStopDateStr
) < 5: # Il faut au moins 5 données pour fitter
DEGENERATE_CASE = True
if i > 0:
DatesString.addOtherDates(fitStartDateStr)
# Récupérations des données observées
dataLengthPeriod = 0
indMinPeriod = (fitStartDate - readStartDate).days
for j, z in enumerate(pd_exerpt.loc[
fitStartDateStr:addDaystoStrDate(fitStopDateStr, -1),
HeadData[0]]):
data[indMinPeriod + j, 0] = z
dataLengthPeriod += 1
for j, z in enumerate(pd_exerpt.loc[
fitStartDateStr:addDaystoStrDate(fitStopDateStr, -1),
HeadData[1]]):
data[indMinPeriod + j, 1] = z
slicedata = slice(indMinPeriod, indMinPeriod + dataLengthPeriod)
slicedataderiv = slice(slicedata.start + 1, slicedata.stop)
if verbose > 0:
print(' dataLength =', dataLength)
print(' indMinPeriod =', indMinPeriod)
print(' dataLengthPeriod=', dataLengthPeriod)
print(' fitStartDateStr =', fitStartDateStr)
print(' fitStopDateStr =', fitStopDateStr)
#input('attente')
# Set initialisation data for the solveur
############################################################################
# paramètres initiaux à optimiser
if i == 0:
datelegend = fitStartDateStr
# ts=getNbDaysBetweenDateFromString(DatesString.listFirstCaseDates[0], readStartDateStr)
# En premiere approximation, on prend la date du premier cas estimé pour le pays (même si c'est faux pour les régions et dpts)
ts = getNbDaysBetweenDateFromString(
DatesString.listFirstCaseDates[0], dateFirstNonZeroStr)
if ts < 0:
continue # On passe au pays suivant
if nbperiodes != 1: # pour plusieurs périodes
#l, b0, c0, f0 = 0.255, 1./5.2, 1./12, 0.08
#a0 = (l+c0)*(1.+l/b0)
#a0, b0, c0, f0, mu0, xi0 = 0.55, 0.34, 0.12, 0.25, 0.0005, 0.0001
a0, b0, c0, f0, mu0, xi0 = 0.60, 0.55, 0.30, 0.50, 0.0005, 0.0001
T = 150
else: # pour une période
#a0, b0, c0, f0, mu0, xi0 = 0.10, 0.29, 0.10, 0.0022, 0.00004, 0.
a0, b0, c0, f0, mu0, xi0 = 0.70, 0.25, 0.05, 0.003, 0.0005, 0.0001
T = 350
if i == 1 or i == 2:
datelegend = None
_, a0, b0, c0, f0, mu0, xi0 = solveur.modele.getParam()
R10 = int(data[indMinPeriod,
0]) # on corrige R1 à la valeur numérique
F0 = int(data[indMinPeriod,
1]) # on corrige F à la valeur numérique
if i == 1:
a0 /= 4. # le confinement réduit drastiquement (pour aider l'optimisation)
T = 120
ts = 0
time = np.linspace(0, T - 1, T)
solveur.modele.setParam(N=N,
a=a0,
b=b0,
c=c0,
f=f0,
mu=mu0,
xi=xi0)
solveur.setParamInit(N=N, E0=E0, I0=I0, R10=R10, R20=R20, D0=D0)
# Before optimization
###############################
# Solve ode avant optimization
sol_ode = solveur.solveEDO(time)
# calcul time shift initial (ts) with respect to data
if i == 0:
ts = solveur.compute_tsfromEQM(data[slicedata, :], T,
indexdata)
else:
solveur.TS = ts = 0
sliceedo = slice(ts, min(ts + dataLengthPeriod, T))
if verbose > 0:
print(solveur)
print(' ts=' + str(ts))
# plot
if plot == True and DEGENERATE_CASE == False:
commontitre = placefull + '- Period ' + str(i) + '\\' + str(
len(ListDatesStr) - 1
) + ' - [' + fitStartDateStr + '\u2192' + addDaystoStrDate(
fitStopDateStr, -1)
if sewe == 0:
titre = commontitre + '] (Delay (delta)=' + str(
decalage) + ')'
else:
titre = commontitre + '] (Sex=', +sexestr + ', Delay (delta)=' + str(
decalage) + ')'
listePlot = indexdata
filename = prefFig + str(decalage) + '_Period' + str(
i) + '_' + ''.join(map(str, listePlot)) + 'Init.png'
solveur.plotEDO(filename,
titre,
sliceedo,
slicedata,
plot=listePlot,
data=data,
text=solveur.getTextParam(datelegend,
Period=i))
# Parameters optimization
############################################################################
solveur.paramOptimization(
data[slicedata, :],
time) # version lorsque ts est calculé automatiquement
#solveur.paramOptimization(data[slicedata, :], time, ts) # version lorsque l'on veut fixer ts
_, a1, b1, c1, f1, mu1, xi1 = solveur.modele.getParam()
R0 = solveur.modele.getR0()
if verbose > 0:
print('Solver' 's state after optimization=', solveur)
print(' Reproductivité après: ', R0)
# After optimization
###############################
# Solve ode avant optimization
sol_ode = solveur.solveEDO(time)
# calcul time shift with respect to data
if i == 0:
ts = solveur.compute_tsfromEQM(data[slicedata, :], T,
indexdata)
else:
solveur.TS = ts = 0
sliceedo = slice(ts, min(ts + dataLengthPeriod, T))
sliceedoderiv = slice(sliceedo.start + 1, sliceedo.stop)
if verbose > 0:
print(solveur)
print(' ts=' + str(ts))
if i == 0: # on se souvient de la date du premier infesté
dateI0 = addDaystoStrDate(fitStartDateStr, -ts + shift_0value)
if verbose > 2:
print('dateI0=', dateI0)
input('attente')
# sauvegarde des param (tableau et texte)
seuil = (data[slicedata.stop - 1, 0] - data[slicedata.start, 0]
) / getNbDaysBetweenDateFromString(
fitStartDateStr, fitStopDateStr) / N
#print('seuil=', seuil)
#print('DEGENERATE_CASE=', DEGENERATE_CASE)
if DEGENERATE_CASE == True:
ROsignificatif = False
ListetabParamModelPlace.append(
[-1., -1., -1., -1., -1., -1., -1.])
else:
if seuil < thresholdSignif:
ROsignificatif = False
ListetabParamModelPlace.append(
[a1, b1, c1, f1, mu1, xi1, -1.])
else:
ROsignificatif = True
ListetabParamModelPlace.append(
[a1, b1, c1, f1, mu1, xi1, R0])
# print('seuil=', seuil)
# print('ROsignificatif=', ROsignificatif)
# print('R0=', R0)
# input('pause')
ListeTextParamPlace.append(
solveur.getTextParamWeak(datelegend, ROsignificatif, Period=i))
data_deriv_period = (
data[slicedataderiv, :] -
data[slicedataderiv.start - 1:slicedataderiv.stop - 1, :]) / dt
modelR1_deriv_period = (
sol_ode[sliceedoderiv, indexdata] -
sol_ode[sliceedoderiv.start - 1:sliceedoderiv.stop - 1,
indexdata]) / dt
data_all_period = data[slicedataderiv, :]
modelR1_all_period = sol_ode[sliceedoderiv, indexdata]
if plot == True and DEGENERATE_CASE == False:
commontitre = placefull + '- Period ' + str(i) + '\\' + str(
len(ListDatesStr) - 1
) + ' - [' + fitStartDateStr + '\u2192' + addDaystoStrDate(
fitStopDateStr, -1)
if sexe == 0:
titre = commontitre + '] (Delay (delta)=' + str(
decalage) + ')'
else:
titre = commontitre + '] (Sex=', +sexestr + ', Delay (delta)=' + str(
decalage) + ')'
# listePlot = [0,1,2,3,4,5]
# filename = prefFig + str(decalage) + '_Period' + str(i) + '_' + ''.join(map(str, listePlot)) + '.png'
# solveur.plotEDO(filename, titre, sliceedo, slicedata, plot=listePlot, data=data, text=solveur.getTextParam(datelegend, ROsignificatif, Period=i))
listePlot = [1, 2, 3, 5]
filename = prefFig + str(decalage) + '_Period' + str(
i) + '_' + ''.join(map(str, listePlot)) + 'Final.png'
solveur.plotEDO(filename,
titre,
sliceedo,
slicedata,
plot=listePlot,
data=data,
text=solveur.getTextParam(datelegend,
ROsignificatif,
Period=i))
listePlot = indexdata
filename = prefFig + str(decalage) + '_Period' + str(
i) + '_' + ''.join(map(str, listePlot)) + 'Final.png'
solveur.plotEDO(filename,
titre,
sliceedo,
slicedata,
plot=listePlot,
data=data,
text=solveur.getTextParam(datelegend,
ROsignificatif,
Period=i))
# dérivée numérique de R1 et F
filename = prefFig + str(decalage) + '_Period' + str(
i) + '_' + ''.join(map(str, listePlot)) + 'Deriv.png'
solveur.plotEDO_deriv(filename,
titre,
sliceedoderiv,
slicedataderiv,
data_deriv_period,
indexdata,
text=solveur.getTextParam(datelegend,
ROsignificatif,
Period=i))
# sol_ode_withSwitch = solveur.solveEDO_withSwitch(T, timeswitch=ts+dataLengthPeriod)
# ajout des données dérivées
data_all[indexplace, slicedataderiv, :] = data_all_period
modelR1_all[indexplace, slicedataderiv, :] = modelR1_all_period
data_deriv[indexplace, slicedataderiv, :] = data_deriv_period
modelR1_deriv[indexplace, slicedataderiv, :] = modelR1_deriv_period
# ajout des SEIR1R2D
modelSEIR1R2D[indexplace,
slicedata.start:slicedata.stop, :] = sol_ode[
ts:ts + sliceedo.stop - sliceedo.start, :]
# preparation for next iteration
_, E0, I0, R10, R20, D0 = map(
int, sol_ode[ts + dataLengthPeriod + recouvrement, :])
#print('A LA FIN : E0, I0, R10, R20, D0=', E0, I0, R10, R20, D0)
if verbose > 1:
input('next step')
Listepd.append(pd_exerpt)
ListeDateI0.append(dateI0)
# calcul de l'EQM sur les données (et non sur les dérivées des données)
#EQM = mean_squared_error(data_deriv[indexplace, :], modelR1_deriv[indexplace, :])
EQM = mean_squared_error(data_all[indexplace, :],
modelR1_all[indexplace, :])
ListeEQM.append(EQM)
# udpate des listes pour transmission
ListeTextParam.append(ListeTextParamPlace)
ListetabParamModel.append(ListetabParamModelPlace)
return modelSEIR1R2D, ListeTextParam, Listepd, data_deriv, modelR1_deriv, ListetabParamModel, ListeEQM, ListeDateI0
def run_localization(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3,
alpha=.00001,
beta=2,
kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3,
fx=fx,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([0, 0, 0])
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.array([sigma_range**2, sigma_bearing**2])
ukf.Q = np.eye(3) * 0.0001
sim_pos = np.array([0, 0, 0.5])
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, u, dt / step, wheelbase)
track.append(sim_pos)
if i % step == 0:
ukf.predict(fx_args=u)
if i % ellipse_step == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
n = randint(4)
for lmark in landmarks[0:n + 1]:
dx, dy = lmark[0] - x, lmark[1] - y
d = sqrt(dx**2 + dy**2) + randn() * sigma_range
bearing = atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
randn() * sigma_bearing))
z.extend([d, a])
print z
ukf.update(
z,
hx_args=landmarks[0:n + 1],
)
if i % ellipse_step == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
return ukf
def run_sim(measurements, controlIn, truth):
points = MerweScaledSigmaPoints(n=3,
alpha=.00001,
beta=2,
kappa=0,
subtract=residual_x)
dt = 0.1 #0.1s timestep
ukf = UKF(dim_x=3,
dim_z=2,
fx=state_trans,
hx=measurement_trans,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.array([0., -0.3, 0.])
ukf.P = np.diag([.01, .01, .01])
#ukf.R = np.diag([0.1**2, 0.1**2, 0**2])
ukf.R = np.diag(
[0.001**2, 0.001**2]
) #measurement noise = [linkeOdometrie, rechteOdometrie, distanzInCm, angleInRadians]
ukf.Q = np.diag([
0.00001**2, 0.00001**2, 0.001**2
]) #process noise = [xRobo, yRobo, Orientierung, xHindernis, yHindernis]
plt.plot(ukf.x[0], ukf.x[1], 'go', alpha=0.3)
for x in range(np.size(measurements, 0)):
u = controlIn[x]
z = measurements[x][2:3]
#z = measurements[x]
ukf.predict(u=u)
#R hoch stzen bei sensor output 1
#if(z[2] >= 1):
# badR = np.diag([0.1**2, 0.1**2, 10000000**2])
# ukf.update(z, badR, u = u, dt = dt)
#else:
ukf.update(z, u=u, dt=dt)
try:
_ = np.linalg.cholesky(ukf.P)
except np.linalg.LinAlgError:
ukf.P = ps.nearestPD(ukf.P)
angle = -np.pi / 2
rotation = np.array([[cos(angle), -sin(angle)],
[sin(angle), cos(angle)]])
if x < 10000:
#roboX = ukf.x[0] * rotation[0][0] + ukf.x[1] * rotation[0][1]
#roboY = ukf.x[0] * rotation[1][0] + ukf.x[1] * rotation[1][1]
#obsX = ukf.x[3] * rotation[0][0] + ukf.x[4] * rotation[0][1]
#obsY = ukf.x[3] * rotation[1][0] + ukf.x[4] * rotation[1][1]
#plt.plot(roboX, roboY, 'go', alpha=0.3)
#plt.plot(obsX, obsY, 'bo', alpha=0.3)
plt.plot(ukf.x[0], ukf.x[1], 'go', alpha=0.3)
#plt.plot(ukf.x[3], ukf.x[4], 'bo', alpha=0.3)
#print(ukf.x[2])
#echte position roboter
plt.plot(truth[x][0], truth[x][1], 'ko', alpha=0.3)
#plot_covariance((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=.1, facecolor='g', alpha=0.3)
print(ukf.x)
#echte position hindernis
plt.plot(0, 0.25, 'ro', alpha=0.3)
plt.show()
py = y + np.random.randn() * r
if n < 1:
x0 = x
y0 = y
zs.append([x, y])
else:
zs.append([px, py])
if setv:
setv = False
vx0 = (x - x0) / dt
vy0 = (y - y0) / dt
print("x0: %6.2f y0: %6.2f vx0: %5.2f vy0: %5.2f" %
(x0, y0, vx0, vy0))
# build a ca filter for comparation
points_ca = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=0.1)
ukf_ca = UnscentedKalmanFilter(dim_x=6,
dim_z=2,
fx=f_ca,
hx=h_ca,
dt=dt * 1.1,
points=points_ca)
# x vx ax y vy ay
vstart_ca = [x0, vx0, 0., y0, vy0, -60.]
ukf_ca.x = np.array(vstart_ca)
ukf_ca.R = np.diag([r * r / 2., r * r / 2.])
ukf_ca.Q[0:3, 0:3] = Q_discrete_white_noise(3, dt=dt, var=0.001)
ukf_ca.Q[3:6, 3:6] = Q_discrete_white_noise(3, dt=dt, var=0.001)
ukf_ca.P *= 2.
flat_inertia0 = array([0, 0, 1, 0, 1])
state0 = hstack([eulers, angular_velocity0, flat_inertia0])
elif case == 3:
DIM_X = 11
stf = state_transition_function_eigens_and_eulers
flat_inertia0 = array([1, 1])
euler_offsets = array([0, 0, 0])
state0 = hstack(
[eulers, angular_velocity0, flat_inertia0, euler_offsets])
elif case == 4:
DIM_X = 6
stf = state_transition_function_const_omega
state0 = hstack([eulers, angular_velocity0])
DIM_Z = 1
points = MerweScaledSigmaPoints(DIM_X, alpha=.3, beta=2, kappa=.1)
lightcurve = load('true_lightcurve.npy')
reverse_lightcurve = flip(lightcurve.copy())
time = load('time.npy')
kf = UnscentedKalmanFilter(dim_x=DIM_X,
dim_z=DIM_Z,
dt=DT,
fx=stf,
hx=measurement_function,
points=points)
kf.x = state0
kf.P = 1
z_std = MEASUREMENT_VARIANCE
R = z_std**2
def reset(self):
"""
Reset our SIRD model.
"""
# Reset β, γ and μ to the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
Model.__DATA = None
self.__beta = 0.4
self.__gamma = 0.035
self.__mu = 0.005
# Reset I, R and D to the data at day 0 or the values mentioned on Wikipedia (see https://bit.ly/2VMvb6h).
if self.__use_data:
self.__x = np.array(
[self.__data_i(0),
self.__data_r(0),
self.__data_d(0)])
self.__n = self.__population
else:
self.__x = np.array([3, 0, 0])
self.__n = 1000
# Reset our Unscented Kalman filter (if required). Note tat we use a dt value of 1 (day) and not the value of
# Model.__DELTA_T.
if self.__use_data:
points = MerweScaledSigmaPoints(
Model.__N_FILTERED,
1e-3, # Alpha value (usually a small positive value like 1e-3).
2, # Beta value (a value of 2 is optimal for a Gaussian distribution).
0, # Kappa value (usually, either 0 or 3-n).
)
self.__ukf = UnscentedKalmanFilter(Model.__N_FILTERED,
Model.__N_MEASURED, 1, self.__h,
Model.__f, points)
self.__ukf.x = np.array([
self.__data_i(0),
self.__data_r(0),
self.__data_d(0), self.__beta, self.__gamma, self.__mu,
self.__n
])
self.__ukf.P *= 15
# Reset our data (if requested).
if self.__use_data:
self.__data_s_values = np.array([self.__data_s(0)])
self.__data_i_values = np.array([self.__data_i(0)])
self.__data_r_values = np.array([self.__data_r(0)])
self.__data_d_values = np.array([self.__data_d(0)])
# Reset our predicted/estimated values.
self.__s_values = np.array([self.__s_value()])
self.__i_values = np.array([self.__i_value()])
self.__r_values = np.array([self.__r_value()])
self.__d_values = np.array([self.__d_value()])
# Reset our estimated SIRD model parameters.
self.__beta_values = np.array([self.__beta])
self.__gamma_values = np.array([self.__gamma])
self.__mu_values = np.array([self.__mu])
def run_localization(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3,
alpha=.00001,
beta=2,
kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3,
dim_z=2 * len(landmarks),
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.append(np.mean(landmarks, axis=0),
(np.mean(cmds, axis=0)[1])) # set initial guess to mean
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks))
ukf.Q = np.eye(3) * 0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, dt / step, u, wheelbase)
print('Sim position:', sim_pos)
track.append(sim_pos)
if i % step == 0:
ukf.predict(u=u, wheelbase=wheelbase)
if i % ellipse_step == 0:
plot_covariance((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = math.sqrt(dx**2 + dy**2) + np.random.randn() * sigma_range
bearing = math.atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
np.random.randn() * sigma_bearing))
z.extend([d, a])
ukf.update(z, landmarks=landmarks)
if i % ellipse_step == 0:
plot_covariance((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.title("UKF Robot localization")
plt.xlim(0, 4.2)
plt.ylim(0, 6.05)
plt.savefig('kalman_filter/%d.png' % len(os.listdir('kalman_filter/')))
plt.close()
return ukf
def show_sigma_selections():
ax = plt.gca()
ax.axes.get_xaxis().set_visible(False)
ax.axes.get_yaxis().set_visible(False)
x = np.array([2, 5])
P = np.array([[3, 1.1], [1.1, 4]])
points = MerweScaledSigmaPoints(2, .09, 2., 1.)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
x = np.array([5, 5])
points = MerweScaledSigmaPoints(2, .15, 1., .15)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
x = np.array([8, 5])
points = MerweScaledSigmaPoints(2, .2, 3., 10)
sigmas = points.sigma_points(x, P)
Wm, Wc = points.weights()
plot_covariance_ellipse(x, P, facecolor='b', alpha=0.3, variance=[.5])
_plot_sigmas(sigmas, Wc, alpha=1.0, facecolor='k')
plt.axis('equal')
plt.xlim(0, 10)
plt.ylim(0, 10)
plt.show()
return [
measurement.latitude,
measurement.longitude,
measurement.elevation,
measurement.heading,
measurement.velocity,
measurement.acceleration,
]
dim_x = 6
dim_z = 6
dt = 1
# points=[[0,0,1,1]]
points = MerweScaledSigmaPoints(6, alpha=.1, beta=2., kappa=-1)
kalaman_filter = UnscentedKalmanFilter(dim_x,
dim_z,
dt,
hx,
fx,
points,
x_mean_fn=state_mean,
residual_x=residual)
gpx = load_gpx_file('data/track_2018-03-14_134847.gpx')
measurements = list(extract_gps_measurements(gpx))
kalaman_filter.x = numpy.array(format_measurement(
measurements[0])) # initial state
kalaman_filter.P *= 0.1 # initial uncertainty
def pendulum(self):
messagebox.showinfo('Message title',
'End simulation by clicking Esc button')
# visulaization parameters
height = 600
width = 600
# simulation
center = None
# Pendulum parameters and variables
# initial conditions
theta = np.pi / 4 # the angle
omega = 0 # the angular velocity
# parametrs
g = 9.8 # m/s^2
L = .2 # m
m = 0.05 # kg
b = 0.001 # friction constant
# Numerical integration parameters
framerate = 60.0 # in frames per second
dt = 1.0 / framerate # Set dt to match the framerate of the webcam or animation
t = time.clock()
# Drawing parametres
thickness = 3
# Noise parameters
Sigma = 30 * np.array([[1, 0], [0, 1]])
#Kalman inferred state variables
theta_kf = theta
omega_kf = omega
#theta_kf_old = theta_kf
#Keep looping
# Create background image
frame = np.zeros((height, width, 3), np.uint8)
center_old = (300, 300)
center_noisy_old = (300, 300)
center_kf_old = (300, 300)
L_kf = 200
# Create background image
frame = np.zeros((height, width, 3), np.uint8)
cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1)
##################
# ukf functions #
##################
#function to return the nonlinear state transition vatiables (theta, omega)
def fx(X, dt):
theta = X[0]
omega = X[1]
theta = theta + omega * dt
omega = omega - dt * g / L_kf * np.sin(theta) - dt * b / (
m * L_kf * L_kf) * omega
return np.c_[theta, omega]
# The update step converts the sigmas into measurement space via the h(x) ,return theta and omega function[https://share.cocalc.com/share/7557a5ac1c870f1ec8f01271959b16b49df9d087/Kalman-and-Bayesian-Filters-in-Python/10-Unscented-Kalman-Filter.ipynb?viewer=share]
def hx(X):
return X
points = MerweScaledSigmaPoints(2, alpha=1e-3, beta=2., kappa=4)
#points = JulierSigmaPoints(n=2, kappa=1)
kf = UKF(dim_x=2, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([theta_kf, omega_kf]) # initial state
kf.R = Sigma # a measurement noise matrix
kf.Q = np.diag(
[4, 4]) # process noise the smae shape as the state variables 2X2
######################
# end ukf functions #
######################
global readings_noisy, readings_after_ukf, theta_theoritical
readings_noisy = []
readings_after_ukf = []
theta_theoritical = []
while True:
cv2.circle(frame, (300, 300), 10, (0, 255, 255), -1)
# == Simulation model ==
# Update state
theta = theta + dt * omega
theta_theoritical.append(theta)
omega = omega - dt * g / L * np.sin(theta) - dt * b / (m * L *
L) * omega
# Map the state to a nearby pixel location
center = np.array((int(300 + 200 * np.sin(theta)),
int(300 + 200 * np.cos(theta))))
center_noisy = tuple(
center + np.matmul(Sigma, np.random.randn(2)).astype(int))
# Draw the pendulum
cv2.circle(frame, tuple(center_old), 10, (0, 0, 0), -1)
cv2.circle(frame, center_noisy_old, 10, (0, 0, 0), -1)
cv2.circle(frame, tuple(center), 10, (0, 255, 255), -1)
cv2.circle(frame, center_noisy, 10, (0, 0, 255), -1)
center_old = center
center_noisy_old = center_noisy
####################################################################
#### here starts the unscented kalman filter implementation #
####################################################################
center_noisy_kf = center_noisy
readings_noisy.append(
np.arctan(
(center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300)))
print('theoritical theta and omega', theta, omega)
#unscented kalman filter pridection
kf.predict()
#print('predicted theta and omega',kf.x[0],kf.x[1])
theta_kf = kf.x[0]
omega_kf = kf.x[1]
print('predicted theta and omega', theta_kf, omega_kf)
#center noisy kf update
# unscented kalman filter updating the state variables
kf.update([(np.arctan(
(center_noisy_kf[0] - 300) / (center_noisy_kf[1] - 300))), 0])
#print("after updtae",theta_kf)
#maping the updated theta to the nearest pixels
center_kf = np.array((int(300 + L_kf * np.sin(kf.x[0])),
int(300 + L_kf * np.cos(kf.x[0]))))
readings_after_ukf.append(
np.arctan((center_kf[0] - 300) / (center_kf[1] - 300)))
####################################################################
#### here finishes the unscented kalman filter implementation #
####################################################################
# Map the state to a nearby pixel location
cv2.circle(frame, tuple(center_kf_old), 10, (0, 0, 0), -1)
center_kf_old = center_kf
cv2.circle(frame, tuple(center_kf), 10, (255, 0, 255), -1)
# show the frame to our screen
cv2.imshow("Frame", frame)
key = cv2.waitKey(int(dt * 400)) & 0xFF
#if the 'Esc' key is pressed, stop the loop
if key == 27:
break
# Wait with calculating next animation step to match the intended framerate
t_ready = time.clock()
d_t_animation = t + dt - t_ready
t += dt
if d_t_animation > 0:
time.sleep(d_t_animation)
# close all windows
cv2.destroyAllWindows()
[0 , 1]]
H (measurement function):
[c, (1 - a - b)]^T
Returns:
UKF.UnscentedKalmanFilter: 2D UKF
"""
assert all(np.isin(["a", "b", "c"], [k for k in params.keys()]))
a, b, c = params["a"], params["b"], params["c"]
# generate sigma points - van der Merwe method
# n = number of dimensions; alpha = how spread out;
# beta = prior knowledge about distribution, 2 == gaussian;
# kappa = scaling parameter, either 0 or 3-n;
points = MerweScaledSigmaPoints(n=2, alpha=alpha, beta=beta, kappa=kappa)
def fx2d(x, dt):
a, b, c = params["a"], params["b"], params["c"]
F = np.array([[1 - c, a], [0.0, 1.0]])
x = np.dot(F, x)
return x
def hx2d(prior_sigma, P_matrix=False):
a, b, c = params["a"], params["b"], params["c"]
H = np.array([[c, (1 - a - b)], [0, 1]])
if P_matrix:
z_sigma = (H @ prior_sigma) @ np.transpose(
H
) # np.dot(np.dot(H, prior_sigma), np.transpose(H))
else: # default
def test_linear_rts():
""" for a linear model the Kalman filter and UKF should produce nearly
identical results.
Test code mostly due to user gboehl as reported in GitHub issue #97, though
I converted it from an AR(1) process to constant velocity kinematic
model.
"""
dt = 1.0
F = np.array([[1., dt], [.0, 1]])
H = np.array([[1., .0]])
def t_func(x, dt):
F = np.array([[1., dt], [.0, 1]])
return np.dot(F, x)
def o_func(x):
return np.dot(H, x)
sig_t = .1 # peocess
sig_o = .00000001 # measurement
N = 50
X_true, X_obs = [], []
for i in range(N):
X_true.append([i + 1, 1.])
X_obs.append(i + 1 + np.random.normal(scale=sig_o))
X_true = np.array(X_true)
X_obs = np.array(X_obs)
oc = np.ones((1, 1)) * sig_o**2
tc = np.zeros((2, 2))
tc[1, 1] = sig_t**2
tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2)
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1)
ukf = UKF(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points)
ukf.x = np.array([0., 1.])
ukf.R = oc[:]
ukf.Q = tc[:]
s = Saver(ukf)
s.save()
s.to_array()
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([[0., 1]]).T
kf.R = oc[:]
kf.Q = tc[:]
kf.H = H[:]
kf.F = F[:]
mu_ukf, cov_ukf = ukf.batch_filter(X_obs)
x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf)
mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs)
x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf)
# check results of filtering are correct
kfx = mu_kf[:, 0, 0]
ukfx = mu_ukf[:, 0]
kfxx = mu_kf[:, 1, 0]
ukfxx = mu_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
# error in position should be smaller then error in velocity, hence
# atol is different for the two tests.
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
# now ensure the RTS smoothers gave nearly identical results
kfx = x_kf[:, 0, 0]
ukfx = x_ukf[:, 0]
kfxx = x_kf[:, 1, 0]
ukfxx = x_ukf[:, 1]
dx = kfx - ukfx
dxx = kfxx - ukfxx
assert np.allclose(dx, 0, atol=1e-7)
assert np.allclose(dxx, 0, atol=1e-6)
return ukf
def __init__(self,
x,
P,
Q,
R,
mode="ore",
enable_emp=True,
enable_bias=True,
output_debug=False):
self.n_total = len(x) # total number of dimensions
self.n_bias = len(np.diag(R)) # total number of biases to estimate
self.n_sensor = len(np.diag(R)) # total number of sensor measurements
if not enable_bias:
self.n_bias = 0
self.emp = enable_emp
self.bias = enable_bias
self.output_debug = output_debug
# Set up filterpy UKF
self.ukf = UnscentedKalmanFilter(
self.n_total, self.n_sensor, 120, lambda x: self.hx(x),
lambda x, dt: self.fx(x, dt),
MerweScaledSigmaPoints(self.n_total, 1e-3, 2, 0),
scipy.linalg.cholesky, None, None, None, None)
# set up debug publisher
if self.output_debug:
self.debug_pub = rospy.Publisher("state",
FilterState,
queue_size=10)
# set initial filter states
self.ukf.x = x
self.ukf.P = P
self.ukf.R = R
self.ukf.Q = np.diag(
np.concatenate([np.diag(Q), np.zeros(self.n_bias + 3)]))
# set timestamp of initial filter state
self.t_ukf = 0.0
self.integration_step = 100.0
self.mode = mode
self.stm_matrix = RelativeOrbitalSTM()
# time constant of empirical accelerations model
self.tau_emp = 400
self.t_oe_c = 0.0
# variables to store noisy chaser state
# and target states (target is only used for evaluation)
self.O_c = rsl.KepOrbElem()
self.osc_O_c = rsl.OscKepOrbElem()
self.osc_O_t = rsl.OscKepOrbElem()
# variables to store exact chaser state
self.O_c_exact = rsl.KepOrbElem()
self.osc_O_c_exact = rsl.OscKepOrbElem()
self.R_J2K_CB = None
# constant term for J2
self.J2_term = (3.0 / 4.0) * (rsl.Constants.J_2 *
(rsl.Constants.R_earth**2) *
np.sqrt(rsl.Constants.mu_earth))
self.update_lock = Lock()
if self.output_debug:
print "SETUP DONE"
mean = (0, 0)
p = np.array([[32, 15], [15., 40.]])
# Compute linearized mean
mean_fx = f_nonlinear_xy(*mean)
#generate random points
xs, ys = multivariate_normal(mean=mean, cov=p, size=10000).T
#initial mean and covariance
mean = (0, 0)
p = np.array([[32., 15], [15., 40.]])
# create sigma points - we will learn about this later
points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, p)
### pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1])
### use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
#generate random points
np.random.seed(100)
xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T
from kf_book.ukf_internal import plot_radar, plot_altitude
dt = 3. # 12 seconds between readings
range_std = 5 # meters
elevation_angle_std = math.radians(0.5)
ac_pos = (0., 1000.)
radar_pos = (0, 0)
ac_vel = (100., 0.)
h_radar.radar_pos = radar_pos
np.random.seed(200)
radar = RadarStation(radar_pos, range_std, elevation_angle_std)
ac = ACSim(ac_pos, (100, 0), 0.02)
time = np.arange(0, 360 + dt, dt)
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=0.)
P = np.zeros((4, 4)) # 4 element in state
Q = np.zeros((4, 4)) # 4 element in state # shape: 4*4
Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.1)
P = np.diag([300**2, 3**2, 150**2, 3**2])
R_std = [range_std**2, elevation_angle_std**2]
R = np.diag(R_std) # shape: 2*2
def myUKF(fx, hx, P, Q, R):
points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1.)
kf = UKF(4, 2, dt, fx=fx, hx=hx,
points=points) #(x_dimm, z_dimm,dt, hx, fx, sigmaPoints)
kf.P = P
def __init__(self, modelType, deltaT, measurementNoiseStd):
"""
Initialises a tracker using initial bounding box.
"""
self.measurementNoiseStd = measurementNoiseStd
self.modelType = modelType
if (modelType == 0): #use contant linear velocity model
# x = [x, y, ax, ay]
self.updatedPredictions = []
self.kf = KalmanFilter(dim_x=4, dim_z=2)
self.kf.F = np.array([[1, 0, deltaT, 0], [0, 1, 0, deltaT],
[0, 0, 1, 0], [0, 0, 0, 1]])
self.kf.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
self.kf.x = np.array([0.01, 0.01, 0.01, 0.01])
self.kf.P *= measurementNoiseStd**2
self.kf.Q *= 0.005
self.kf.R *= measurementNoiseStd**2
elif (modelType == 1):
"""
Constant turn rate model
"""
points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0)
self.updatedPredictions = []
self.kf = UnscentedKalmanFilter(dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_turnRateModel,
hx=h_unscented_turnRateModel,
points=points1)
self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 1e-10])
self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.kf.R = np.eye(2) * (measurementNoiseStd**2)
self.kf.Q = np.diag([1e-24, 1e-24, 1e-3, 4e-3, 1e-10])
elif (modelType == 2):
"""
Constant linear velocity model
"""
points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0)
self.updatedPredictions = []
self.kf = []
self.kf = UnscentedKalmanFilter(dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_linearModel,
hx=h_unscented_linearModel,
points=points1)
self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 0])
self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.kf.R = np.eye(2) * (measurementNoiseStd**2)
self.kf.Q = np.diag([0.003, 0.003, 6e-4, 0.004, 0])
elif (modelType == 3):
"""
Random Motion Model
"""
points1 = MerweScaledSigmaPoints(5, alpha=0.001, beta=2., kappa=0)
self.updatedPredictions = []
self.kf = []
self.kf = UnscentedKalmanFilter(dim_x=5,
dim_z=2,
dt=deltaT,
fx=f_unscented_randomModel,
hx=h_unscented_randomModel,
points=points1)
self.kf.x = np.array([1e-3, 1e-3, 1e-3, 1e-5, 0])
self.kf.P = np.eye(5) * (measurementNoiseStd**2) / 2.0
self.kf.R = np.eye(2) * (measurementNoiseStd**2)
self.kf.Q = np.diag([1, 1, 1e-24, 1e-24, 1e-24])
plt.scatter(fxs, fys, marker='.', alpha=0.01, color='k')
plt.scatter(mean_fx[0], mean_fx[1], marker='o', s=100, c='r', label='UT_mean')
plt.scatter(computed_mean_x, computed_mean_y, marker='*',s=120, c='b', label='mean')
plt.ylim([-10, 200])
plt.xlim([-100, 100])
plt.legend(loc='best', scatterpoints=1)
print ('Difference in mean x={:.3f}, y={:.3f}'.format(
computed_mean_x-mean_fx[0], computed_mean_y-mean_fx[1]))
# -------------------------------------------------------------------------------------------
mean = [0, 0] # Mean of the N-dimensional distribution.
cov = [[32, 15], [15, 40]] # Covariance matrix of the distribution.
# create sigma points(2n+1个sigma点)
# uses 3 parameters to control how the sigma points are distributed and weighted
points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)
# Wm, Wc = points.weights()
Wm, Wc = points.Wm, points.Wc
sigmas = points.sigma_points(mean, cov)
# pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1])
# use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
# generate random points
xs, ys = multivariate_normal(mean, cov, size=1000).T # 从二维随机变量的正态分布中产生1000个数据点
plot2(xs, ys, f_nonlinear_xy, ukf_mean)
def init_filter(
S0: float = 5.74,
s_variance: float = 1,
Q00_s_noise: float = 0.01,
R: float = 1,
h: Callable = hx,
f: Callable = abc,
params: Dict = parameters,
alpha: float = 1e-3,
beta: float = 2,
kappa: float = 1,
) -> UKF.UnscentedKalmanFilter:
"""Init the ABC model kalman filter
X (state mean):
[S0]^T
P (state uncertainty):
[s_variance]
ABC Model
F (process transition matrix):
H (measurement function):
Args:
S0 (float, optional): [description]. Defaults to 5.74.
s_variance (float, optional): P_t=0. Defaults to 1.
Q00_s_noise (float, optional): [description]. Defaults to 0.01.
R (float, optional): [description]. Defaults to 1.
h (Callable, optional): [description]. Defaults to hx.
f (Callable, optional): [description]. Defaults to abc.
params (Dict, optional): [description]. Defaults to parameters.
alpha (float, optional): [description]. Defaults to 1e-3.
beta (float, optional): [description]. Defaults to 2.
kappa (float, optional): [description]. Defaults to 1.
Returns:
[UKF.UnscentedKalmanFilter]: Initialised Unscented Kalman Filter
"""
assert all(np.isin(["a", "b", "c"], [k for k in params.keys()]))
a, b, c = params["a"], params["b"], params["c"]
# generate sigma points - van der Merwe method
# n = number of dimensions; alpha = how spread out;
# beta = prior knowledge about distribution, 2 == gaussian;
# kappa = scaling parameter, either 0 or 3-n;
points = MerweScaledSigmaPoints(n=1, alpha=alpha, beta=beta, kappa=kappa)
## TODO: dt = 86400s (1day) (??)
abc_filter = UKF.UnscentedKalmanFilter(
dim_x=1, dim_z=1, dt=1, hx=h, fx=f, points=points
)
# INIT FILTER
# ------- Predict Variables -------
# State Vector (X): storage and rainfall
# (2, 1) = column vector
abc_filter.x = np.array([S0])
# State Covariance (P) initial estimate
# (2, 2) = square matrix
abc_filter.P[:] = np.diag([s_variance])
# Process noise (Q)
# (2, 2) = square matrix
abc_filter.Q = np.diag([Q00_s_noise])
# ------- Update Variables -------
# 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
m = array([[5., 10], [10, 5], [15, 15], [20., 16], [0, 30], [50, 30], [40,
10]])
#m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]])
#m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]])
#m = array([[5., 10], [10, 5]])
#m = array([[5., 10], [10, 5]])
sigma_r = .3
sigma_h = .1 #radians(.5)#np.radians(1)
#sigma_steer = radians(10)
dt = 0.1
wheelbase = 0.5
points = MerweScaledSigmaPoints(n=3,
alpha=.1,
beta=2,
kappa=0,
subtract=residual_x)
#points = JulierSigmaPoints(n=3, kappa=3)
ukf = UKF(dim_x=3,
dim_z=2 * len(m),
fx=fx,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .05])
def hx(x):
# measurement function - convert state into a measurement
# where measurements are [x_pos, y_pos]
# x = [pos_x, pos_y, vel_x, vel_y]
# x[0] = pos_x
# x[1] = pos_y
# x[2] = vel_x
# x[3] = vel_y
return np.array([x[0], x[1]]) # Observation x,y position covariance
dt = 0.1
# create sigma points to use in the filter. This is standard for Gaussian processes
points = MerweScaledSigmaPoints(4, alpha=.001, beta=2., kappa=0)
kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
kf.x = np.array([-1., 1., -1., 1]) # initial state
kf.P *= 0.2 # initial uncertainty
z_std = 0.1
kf.R = np.diag([z_std**2, z_std**2]) # 1 standard
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2)
gt = [[i, i] for ia in range(50)] # ground truth
zs = [[i+np.random.randn()*2.0, i+np.random.randn()*0.2] for i in range(50)] # measurements
for g, z in zip(gt, zs):
kf.predict()
kf.update(z)
print(g, z, kf.x, 'log-likelihood', kf.log_likelihood)
def register_and_filter_once(x, filt_times):
start_frame = 48
end_frame = 12
skip = 1
filepath = "./20210312_3axis/"
pose_data = np.genfromtxt(f'{filepath}point_cloud_pose.txt', delimiter=',')
timestamp_data = np.genfromtxt(f'{filepath}timestamp.txt', delimiter=',')
pose_data = pose_data[start_frame:-end_frame:skip, :]
measurement_data = np.genfromtxt(f'{filepath}registration_measurement.csv',
delimiter=',')
measurement_data = measurement_data[::skip, :]
pose_data[:, 6:10] = measurement_data[::skip, 3:7]
timestamp_data = timestamp_data[start_frame:-end_frame:skip, :]
pose_data = continous_quat(pose_data)
# filepath = "./test_pointcloud/"
# pose_data = np.genfromtxt(f'{filepath}blensor_data_csv.txt', delimiter=',')
# pose_data = np.concatenate((np.zeros((pose_data.shape[0],6)), pose_data), axis=1)
# arange_data = np.arange(pose_data.shape[0]).reshape((pose_data.shape[0],1))
# timestamp_data = np.concatenate((arange_data, arange_data*0.05), axis=1)
# pose_data = pose_data[::skip,:]
# timestamp_data = timestamp_data[::skip,:]
quat0_inv = quat_inv(pose_data[0, 6:10])
for i in range(pose_data.shape[0]):
pose_data[i, 6:10] = quaternion_mul_num(pose_data[i, 6:10], quat0_inv)
# In reverse Order
if filt_times % 2 == 0:
pose_data = pose_data[::-1, :]
timestamp_data = timestamp_data[::-1, :]
measurement_data[:, 0:3] = -measurement_data[::-1, 0:3]
source_quat_zero_raw = pose_data[0, 6:10]
R = 1e-6 * np.diag(np.asarray([1, 1, 1]))
P = 1e-4 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
Q = 1e-8 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
# shrink_list = [3, 5, 7]
# shrink_index = bisect(shrink_list, filt_times)
# P[7:9,:] = P[7:9,:]/(10**shrink_index)
# Q[7:9,:] = Q[7:9,:]/(10**shrink_index)
# P[0:6]*=skip**2
# Q[0:6]*=skip**2
P[0:3] *= 1000
Q[0:3] *= 1000
x_log = np.zeros((pose_data.shape[0], 19))
x_log[0, :] = x
#UKF setting
points = MerweScaledSigmaPoints(11, alpha=.1, beta=2., kappa=-1)
ukf = UKF(dim_x=11, dim_z=3, dt=0.1, fx=ukf_f, hx=ukf_h, points=points)
ukf.x = x[0:11] # initial state
ukf.P = P
ukf.Q = Q
ukf.R = R
x_global = x[11:19]
# w_body = np.zeros((pose_data.shape[0]-1, 3))
# for i in range(w_body.shape[0]):
# w_body[i, :] = get_w_in_body_frame(pose_data[i,6:10], pose_data[i+1,6:10], timestamp_data[i+1, 1] - timestamp_data[i, 1])
source_s_root = 1
now_quat = source_quat_zero_raw
register_log = np.zeros((pose_data.shape[0], 4))
dq_real_log = np.zeros((pose_data.shape[0] - 1, 4))
dq_correct_log = np.zeros((pose_data.shape[0] - 1, 4))
register_log[0, :] = now_quat
loop_start_R = Rotation.from_quat(to_scalar_last(now_quat))
loop_angle_log = np.zeros((pose_data.shape[0], ))
for i in range(pose_data.shape[0] - 1):
source_quat_raw = pose_data[i, 6:10]
target_quat_raw = pose_data[i + 1, 6:10]
source_quat = np.zeros((4, ))
target_quat = np.zeros((4, ))
source_quat[3] = source_quat_raw[0]
source_quat[0:3] = source_quat_raw[1:4]
target_quat[3] = target_quat_raw[0]
target_quat[0:3] = target_quat_raw[1:4]
sourceR = Rotation.from_quat(source_quat)
targetR = Rotation.from_quat(target_quat)
nowR_from_loop_start = targetR * loop_start_R.inv()
nowR_from_loop_start_angle = np.linalg.norm(
nowR_from_loop_start.as_rotvec())
loop_angle_log[i] = nowR_from_loop_start_angle
dR_real = targetR * sourceR.inv()
dq_real = to_scalar_first(dR_real.as_quat())
dq_real_log[i, :] = dq_real
dtime = abs(timestamp_data[i + 1, 1] - timestamp_data[i, 1])
# x_predict, P = ekf_predict(x, P, Q, dtime)
x_predict, P = mekf_predict(x, P, Q, dtime)
# ukf.predict(dt=dtime)
# point to plane ICP
dq_estimate = dR_real.as_quat()
if dq_estimate[3] > 0.5:
dq_estimate = dq_estimate
else:
dq_estimate = -dq_estimate
z_measure = np.zeros((7, ))
z_measure[0:3] = dq_estimate[0:3]
# z_measure[3:7] = quaternion_mul_num(to_scalar_first(dq_estimate), now_quat)
z_measure[3:7] = target_quat_raw
# x_correct, P, z_correct = ekf_update(x, x_predict, P, z_measure, R, dtime)
x_correct, P, z_correct = mekf_update(x_predict, P,
measurement_data[i + 1,
0:3], R, dtime)
# ukf, x_global, _ = ukf_update(ukf, z_measure[3:7], x_global)
# dq_correct = np.asarray([z_correct[0],z_correct[1],z_correct[2],1])
# dR_correct = Rotation.from_quat(dq_correct/np.linalg.norm(dq_correct))
x_correct = mekf_normalize_state(x_correct)
x_log[i + 1, :] = x_correct
x = x_correct
# x_global[0:4] = x_global[0:4]/np.linalg.norm(x_global[0:4])
# x_global[4:8] = x_global[4:8]/np.linalg.norm(x_global[4:8])
# x_log[i+1,0:11] = ukf.x
# x_log[i+1,11:19] = x_global
# now_quat = quaternion_mul_num(to_scalar_first(dq_correct), now_quat)
# now_quat = z_correct[3:7]
# register_log[i+1, :] = now_quat
# dq_correct_log[i,:] = to_scalar_first(dq_correct)
np.savetxt(f"{filepath}mekf_log_ground_truth_{filt_times}.csv",
x_log,
delimiter=',')
# x[0:11] = ukf.x
# x[11:19] = x_global
return x
def ukf_filter_without_register(x, filt_times, filepath, measurement_raw,
timestamp_data):
measurement = copy.deepcopy(measurement_raw)
# In reverse Order
if filt_times % 2 == 0:
measurement = measurement[::-1, :]
timestamp_data = timestamp_data[::-1, :]
for i in range(1, measurement.shape[0]):
measurement[measurement.shape[0] - i,
0:3] = -measurement[measurement.shape[0] - i - 1, 0:3]
# measurement[i, 0:3] = -measurement[i, 0:3]
dt = 0.1
points = MerweScaledSigmaPoints(11, alpha=.1, beta=2., kappa=-1)
ukf = UKF(dim_x=11, dim_z=3, dt=dt, fx=ukf_f, hx=ukf_h, points=points)
ukf.x = x[0:11] # initial state
x_global = x[11:19]
ukf.P *= 0.2 # initial uncertainty
ukf.R = 1e-5 * np.diag(np.asarray([1, 1, 1]))
if filt_times == 1:
ukf.P = 1e-4 * np.diag(
np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1]))
ukf.Q = 1e-8 * np.diag(
np.asarray([0.0001, 0.0001, 1, 1, 1, 1, 10, 10, 1, 1, 1]))
else:
ukf.P = 1e-4 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
ukf.Q = 1e-8 * np.diag(
np.asarray([0.001, 0.001, 0.001, 1, 1, 1, 10, 10, 1, 1, 1]))
# shrink_list = [3, 5, 7]
# shrink_index = bisect(shrink_list, filt_times)
# ukf.P[6:8,:] /= (10**shrink_index)
# ukf.Q[6:8,:] /= (10**shrink_index)
# P[8:11,:] = P[8:11,:]/(10**shrink_index)
# Q[8:11,:] = Q[8:11,:]/(10**shrink_index)
# P[3:7] = P[3:7]/(10**shrink_index)
# Q[3:7] = Q[3:7]/(10**shrink_index)
x_log = np.zeros((measurement.shape[0], 19))
x_log[0, 0:11] = ukf.x
x_log[0, 11:19] = x_global
for i in range(measurement.shape[0] - 1):
dtime = abs(timestamp_data[i + 1, 1] - timestamp_data[i, 1])
ukf.predict(dt=dtime)
# z_measure[3:7] = target_quat_raw
z_measure = measurement[i + 1, 3:7]
ukf, x_global, _ = ukf_update(ukf, z_measure, x_global)
x_global[0:4] = x_global[0:4] / np.linalg.norm(x_global[0:4])
x_global[4:8] = x_global[4:8] / np.linalg.norm(x_global[4:8])
x_log[i + 1, 0:11] = ukf.x
x_log[i + 1, 11:19] = x_global
np.savetxt(f"{filepath}ukf_log_all_{filt_times}.csv", x_log, delimiter=',')
x[0:11] = ukf.x
x[11:19] = x_global
return x
R = np.divide(temp, dim_x)
def transition(state, time_step=time_step):
output = np.zeros_like(state)
cursor = state[:6]
output[:6] = F @ cursor
output[6:] = state[6:]
return output
def observation(state):
return H @ state[:6]
points = MerweScaledSigmaPoints(np.size(states, 0), alpha=1., beta=2., kappa=0)
myukf = ukf(dim_x=dim_x + dim_z,
dim_z=dim_z,
dt=time_step,
fx=transition,
hx=observation,
points=points)
initial_state = states[:, 0]
myukf.x = initial_state
myukf.R = R
myukf.Q = np.eye(dim_x + dim_z)
myukf.Q[:np.size(Q, 0), :np.size(Q, 1)] = Q
# Estimation loop
rng = np.size(states, 1) - training_size
ukf_predict = np.zeros((dim_x + dim_z, rng))
