def test_generate(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
#track = core_Track.Track.generate(TestOperateurMethods.x2, TestOperateurMethods.y2)
track = synth.generate(TestOperateurMethods.x2,
TestOperateurMethods.y2)
track.createAnalyticalFeature("a")
track.operate(Operator.RANDOM, "a", prob, "randx")
track.operate(Operator.RANDOM, "a", prob, "randy")
track.operate(Operator.INTEGRATOR, "randx", "randx")
track.operate(Operator.INTEGRATOR, "randy", "randy")
track.operate(Operator.SCALAR_MULTIPLIER, "randx", 0.5, "noisex")
track.operate(Operator.SCALAR_MULTIPLIER, "randx", 0.5, "noisey")
track.operate(Operator.ADDER, "x", "noisex", "x_noised")
track.operate(Operator.ADDER, "y", "noisey", "y_noised")
kernel = GaussianKernel(31)
track.operate(Operator.FILTER, "x_noised", kernel, "x_filtered")
track.operate(Operator.FILTER, "y_noised", kernel, "y_filtered")
plt.plot(track.getAnalyticalFeature("x"),
track.getAnalyticalFeature("y"), 'k--')
plt.plot(track.getAnalyticalFeature("x_noised"),
track.getAnalyticalFeature("y_noised"), 'b-')
plt.plot(track.getAnalyticalFeature("x_filtered"),
track.getAnalyticalFeature("y_filtered"), 'r-')
plt.show()
def test_random(self):
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
#track = core_Track.Track.generate(TestOperateurMethods.x, TestOperateurMethods.y)
track = synth.generate(x, y)
track.createAnalyticalFeature("a")
track.operate(Operator.RANDOM, ("a", "a"), prob,
("x_noised", "y_noised"))
track.operate(Operator.INTEGRATOR, ("x_noised", "y_noised"))
track.operate(Operator.SCALAR_MULTIPLIER, ("x_noised", "y_noised"),
0.02)
track.operate(Operator.ADDER, ("x_noised", "y_noised"), ("x", "y"))
kernel = GaussianKernel(21)
kernel.setFilterBoundary(True)
track.operate(Operator.FILTER, ("x_noised", "y_noised"), kernel,
("x_filtered", "y_filtered"))
plt.plot(track.getX(), track.getY(), 'k--')
plt.plot(track.getAnalyticalFeature("x_noised"),
track.getAnalyticalFeature("y_noised"), 'b-')
plt.plot(track.getAnalyticalFeature("x_filtered"),
track.getAnalyticalFeature("y_filtered"), 'r-')
plt.show()
def example0():
Stochastics.seed(123)
track1 = Synthetics.generate(0.2, dt=10)
track2 = track1.noise(1)
F = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]])
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
Q = np.array([[1e-8, 0, 0, 0], [0, 1e-8, 0, 0], [0, 0, 2e-4, 0],
[0, 0, 0, 2e-4]])
R = np.array([[1, 0], [0, 1]])
X0 = np.array([[track2[0].position.getX()], [track2[0].position.getY()],
[0], [0]])
P0 = np.array([[3, 0, 0, 0], [0, 3, 0, 0], [0, 0, 1e1, 0], [0, 0, 0, 1e1]])
UKF = Kalman()
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track2, ["x", "y"])
track1.plot('k--')
plt.plot(track2['kf_0'], track2['kf_1'], 'b-')
track2.plot('r+')
plt.show()
def example1(mode='FDTW', N=5):
track = Synthetics.generate(1e-1, dt=30)
tracks = NoiseProcess(5, GaussianKernel(5)).noise(track, N)
central = Comparison.centralTrack(tracks, mode=mode)
tracks.plot('r-')
central.plot('b-')
plt.show()
def example0():
track = Synthetics.generate(x_t, y_t, dt=5)
Interpolation.resample(track, 0.1, 1, 1)
track = Stochastics.noise(track, 0.02)
circle = Geometrics.fitCircle(track)
circle.plot()
plt.plot(track.getX(), track.getY(), 'b+')
plt.show()
def example0(mode='FDTW'):
Stochastics.seed(1234)
track1 = Synthetics.generate(2e-1, dt=30)
track2 = track1.noise(10, GaussianKernel(25))
track1.plot('r-')
track2.plot('r-')
profile = Comparison.differenceProfile(track2, track1, mode, p=2)
Comparison.plotDifferenceProfile(profile, track1)
plt.show()
def example0():
track1 = Synthetics.generate(1e-1, dt=30)
track2 = track1.noise(10, GaussianKernel(25))
S = lambda track, k: [
p for p in range(max(0, k - 30), min(len(track2) - 1, k + 30))
]
#S = lambda t, k: [p for p in range(0, len(track2)-1)]
Q = lambda i, j, k, t: (j < i + 30) * (j >= i) * 1
P = lambda s, y, k, t: math.exp(-track2[s].position.distance2DTo(y))
Dynamics.HMM(S, Q, P).estimate(track1, ["x", "y"],
mode=Dynamics.MODE_OBS_AS_2D_POSITIONS,
verbose=2)
track1.plot('r-')
track2.plot('b-')
Comparison.plotDifferenceProfile(track1, track2, "hmm_inference")
plt.show()
def example1(withBiases=False, withCycleSlip=False):
Stochastics.seed(123)
track_gt = Synthetics.generate(0.15)
B1 = [-20, 30]
B2 = [30, 10]
b1 = 0 + withBiases * 25
b2 = 0 + withBiases * 32
sp = 0 + withCycleSlip * 10
track_odo = track_gt.copy()
track_odo = Stochastics.NoiseProcess(2,
GaussianKernel(10)).noise(track_odo)
track_odo.op("dx=D(x)")
track_odo.op("dy=D(y)")
track = track_gt.noise(0.5)
track.biop(track_odo, "vx=dx°")
track.biop(track_odo, "vy=dy°")
externals = {
"x1": B1[0],
"y1": B1[1],
"x2": B2[0],
"y2": B2[1],
"b1": b1,
"b2": b2,
"sp": sp
}
track.op("balise1 = SQRT((x-x1)^2+(y-y1)^2)-b1-sp*(idx>50)", externals)
track.op("balise2 = SQRT((x-x2)^2+(y-y2)^2)-b2", externals)
track = track > 1
track_gt.plot('k--')
plt.plot(B1[0], B1[1], 'k^')
plt.plot(B2[0], B2[1], 'k^')
track.plot('b+')
def F(x, k, track):
return np.array([[x[0, 0] + track["vx", k]],
[x[1, 0] + track["vy", k]], [x[2, 0]], [x[3, 0]]])
def H(x):
return np.array(
[[((x[0, 0] - B1[0])**2 + (x[1, 0] - B1[1])**2)**0.5] - x[2, 0],
[((x[0, 0] - B2[0])**2 + (x[1, 0] - B2[1])**2)**0.5] - x[3, 0]])
Q = 1e-1 * np.eye(4, 4)
Q[2, 2] = 1e-10
Q[3, 3] = 1e-10
R = 0.5**2 * np.eye(2, 2)
X0 = np.array([[-50], [40], [0], [0]])
P0 = 1e1 * np.eye(4, 4)
P0[2, 2] = 1e2
P0[3, 3] = 1e2
UKF = Kalman()
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track, ["balise1", "balise2"],
mode=Dynamics.MODE_STATES_AS_2D_POSITIONS)
track.plot('r-')
# Innovation
#fig, ax1 = plt.subplots(figsize=(5, 3))
#ax1.plot(track["kf_balise1_inov"], 'r-')
#ax1.plot(track["kf_balise2_inov"], 'g-')
#ax1.plot(np.array(track["x"])-np.array(track_gt["x"])[1:], 'b-')
#ax1.plot(np.array(track["y"])-np.array(track_gt["y"])[1:], 'b-')
# Statistics
#print("Std gps =", '{:3.2f}'.format(track.operate(Operator.RMSE,"kf_balise1_inov")), "m")
#print("Std cam =", '{:3.2f}'.format(track.operate(Operator.RMSE,"kf_balise2_inov")), "m")
fig, ax1 = plt.subplots(figsize=(5, 3))
#ax1.plot(track["kf_2"], 'r.')
#ax1.plot(track["kf_3"], 'g.')
ax1.plot(track["kf_balise1_inov"], 'r-')
ax1.plot(track["kf_balise2_inov"], 'g-')
plt.show()