def example3():
start = GeoCoords(2.4320023, 48.84298, 100).toECEFCoords()
print(start)
path = "data/psr.dat"
track = Track()
for i in range(47):
track.addObs(Obs(ECEFCoords(0, 0, 0)))
track.createAnalyticalFeature("m0", [0] * 47)
track.createAnalyticalFeature("sx0", [0] * 47)
track.createAnalyticalFeature("sy0", [0] * 47)
track.createAnalyticalFeature("sz0", [0] * 47)
track.createAnalyticalFeature("m1", [0] * 47)
track.createAnalyticalFeature("sx1", [0] * 47)
track.createAnalyticalFeature("sy1", [0] * 47)
track.createAnalyticalFeature("sz1", [0] * 47)
track.createAnalyticalFeature("m2", [0] * 47)
track.createAnalyticalFeature("sx2", [0] * 47)
track.createAnalyticalFeature("sy2", [0] * 47)
track.createAnalyticalFeature("sz2", [0] * 47)
track.createAnalyticalFeature("m3", [0] * 47)
track.createAnalyticalFeature("sx3", [0] * 47)
track.createAnalyticalFeature("sy3", [0] * 47)
track.createAnalyticalFeature("sz3", [0] * 47)
track.createAnalyticalFeature("m4", [0] * 47)
track.createAnalyticalFeature("sx4", [0] * 47)
track.createAnalyticalFeature("sy4", [0] * 47)
track.createAnalyticalFeature("sz4", [0] * 47)
with open(path) as fp:
line = True
for i in range(47):
for j in range(5):
line = fp.readline()
vals = line[:-2].split(",")
track.setObsAnalyticalFeature("sx" + str(j), i, float(vals[1]))
track.setObsAnalyticalFeature("sy" + str(j), i, float(vals[2]))
track.setObsAnalyticalFeature("sz" + str(j), i, float(vals[3]))
track.setObsAnalyticalFeature("m" + str(j), i, float(vals[4]))
line = fp.readline()
def F(x):
plan = ECEFCoords(x[0, 0], x[1, 0], x[2, 0]).toENUCoords(start)
plan.E += x[5, 0] * math.sin(x[4, 0])
plan.N += x[5, 0] * math.cos(x[4, 0])
xyz = plan.toECEFCoords(start)
return np.array([[xyz.X], [xyz.Y], [xyz.Z], [x[3, 0] + x[6, 0]],
[x[4, 0]], [x[5, 0]], [x[6, 0]]])
def H(x, k, track):
return np.array([
[((x[0, 0] - track["sx0", k])**2 + (x[1, 0] - track["sy0", k])**2 +
(x[2, 0] - track["sz0", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx1", k])**2 + (x[1, 0] - track["sy1", k])**2 +
(x[2, 0] - track["sz1", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx2", k])**2 + (x[1, 0] - track["sy2", k])**2 +
(x[2, 0] - track["sz2", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx3", k])**2 + (x[1, 0] - track["sy3", k])**2 +
(x[2, 0] - track["sz3", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx4", k])**2 + (x[1, 0] - track["sy4", k])**2 +
(x[2, 0] - track["sz4", k])**2)**0.5 + x[3, 0]]
])
Q = 1e0 * np.eye(7, 7)
Q[3, 3] = 0
Q[4, 4] = 1e-10
Q[5, 5] = 1e-1
Q[6, 6] = 1e-1
R = 1e1 * np.eye(5, 5)
X0 = np.array([[start.getX()], [start.getY()], [start.getZ()], [0], [0],
[0], [0]])
P0 = 1e5 * np.eye(7, 7)
P0[3, 3] = 1e6
P0[4, 4] = 1e1
P0[5, 5] = 1e1
P0[6, 6] = 1e3
UKF = Kalman(spreading=1)
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track, ["m0", "m1", "m2", "m3", "m4"],
mode=Dynamics.MODE_STATES_AS_3D_POSITIONS)
track.toGeoCoords()
track.plot('r-')
plt.show()
KmlWriter.writeToKml(track, path="couplage.kml", type="LINE")
def example4():
start = GeoCoords(2.4320023, 48.84298, 100).toECEFCoords()
path = "data/psr_all.dat"
track = Track()
Nepochs = 534
for i in range(Nepochs):
track.addObs(Obs(ECEFCoords(0, 0, 0)))
track.createAnalyticalFeature("m0", [0] * Nepochs)
track.createAnalyticalFeature("sx0", [0] * Nepochs)
track.createAnalyticalFeature("sy0", [0] * Nepochs)
track.createAnalyticalFeature("sz0", [0] * Nepochs)
track.createAnalyticalFeature("m1", [0] * Nepochs)
track.createAnalyticalFeature("sx1", [0] * Nepochs)
track.createAnalyticalFeature("sy1", [0] * Nepochs)
track.createAnalyticalFeature("sz1", [0] * Nepochs)
track.createAnalyticalFeature("m2", [0] * Nepochs)
track.createAnalyticalFeature("sx2", [0] * Nepochs)
track.createAnalyticalFeature("sy2", [0] * Nepochs)
track.createAnalyticalFeature("sz2", [0] * Nepochs)
track.createAnalyticalFeature("m3", [0] * Nepochs)
track.createAnalyticalFeature("sx3", [0] * Nepochs)
track.createAnalyticalFeature("sy3", [0] * Nepochs)
track.createAnalyticalFeature("sz3", [0] * Nepochs)
track.createAnalyticalFeature("m4", [0] * Nepochs)
track.createAnalyticalFeature("sx4", [0] * Nepochs)
track.createAnalyticalFeature("sy4", [0] * Nepochs)
track.createAnalyticalFeature("sz4", [0] * Nepochs)
track.createAnalyticalFeature("m5", [0] * Nepochs)
track.createAnalyticalFeature("sx5", [0] * Nepochs)
track.createAnalyticalFeature("sy5", [0] * Nepochs)
track.createAnalyticalFeature("sz5", [0] * Nepochs)
with open(path) as fp:
line = True
for i in range(Nepochs):
for j in range(6):
line = fp.readline()
vals = line[:-1].split(",")
track.setObsAnalyticalFeature("sx" + str(j), i, float(vals[1]))
track.setObsAnalyticalFeature("sy" + str(j), i, float(vals[2]))
track.setObsAnalyticalFeature("sz" + str(j), i, float(vals[3]))
track.setObsAnalyticalFeature("m" + str(j), i, float(vals[4]))
line = fp.readline()
track = track % [False, True]
def F(x):
return np.array([[x[0, 0]], [x[1, 0]], [x[2, 0]], [x[3, 0] + x[4, 0]],
[x[4, 0]]])
def H(x, k, track):
return np.array([
[((x[0, 0] - track["sx0", k])**2 + (x[1, 0] - track["sy0", k])**2 +
(x[2, 0] - track["sz0", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx1", k])**2 + (x[1, 0] - track["sy1", k])**2 +
(x[2, 0] - track["sz1", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx2", k])**2 + (x[1, 0] - track["sy2", k])**2 +
(x[2, 0] - track["sz2", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx3", k])**2 + (x[1, 0] - track["sy3", k])**2 +
(x[2, 0] - track["sz3", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx4", k])**2 + (x[1, 0] - track["sy4", k])**2 +
(x[2, 0] - track["sz4", k])**2)**0.5 + x[3, 0]],
[((x[0, 0] - track["sx5", k])**2 + (x[1, 0] - track["sy5", k])**2 +
(x[2, 0] - track["sz5", k])**2)**0.5 + x[3, 0]]
])
Q = 1e0 * np.eye(5, 5)
Q[3, 3] = 0
Q[4, 4] = 1e0
def R(k):
P = 1e1 * np.eye(6, 6)
if (k >= 70) and (k < 267):
for i in range(3, 6):
P[i, i] = 1e16
return P
for k in range(70, 267):
for i in range(3, 6):
track.setObsAnalyticalFeature("m" + str(i), k, 20000000)
X0 = np.array([[start.X], [start.Y], [start.Z], [0], [0]])
P0 = 1e5 * np.eye(5, 5)
P0[3, 3] = 1e8
P0[4, 4] = 1e6
UKF = Kalman(spreading=1)
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.summary()
UKF.estimate(track, ["m0", "m1", "m2", "m3", "m4", "m5"],
mode=Dynamics.MODE_STATES_AS_3D_POSITIONS)
track.toGeoCoords()
KmlWriter.writeToKml(track,
path="couplage.kml",
type="POINT",
c1=[0, 1, 0, 1])
track.plot('r+')
plt.show()
class TestAlgoGeometricsMethods(unittest.TestCase):
__epsilon = 0.001
def setUp(self):
#----------------------------------------------------------------------
# 4 sommets sur axes du cercle trigonométrique
GPSTime.setReadFormat("4Y-2M-2D 2h:2m:2s")
self.trace1 = Track()
c1 = ENUCoords(1, 0, 0)
p1 = Obs(c1, GPSTime.readTimestamp("2018-01-01 10:00:00"))
self.trace1.addObs(p1)
c2 = ENUCoords(0, 1, 0)
p2 = Obs(c2, GPSTime.readTimestamp("2018-01-01 10:00:12"))
self.trace1.addObs(p2)
c3 = ENUCoords(-1, 0, 0)
p3 = Obs(c3, GPSTime.readTimestamp("2018-01-01 10:00:40"))
self.trace1.addObs(p3)
c4 = ENUCoords(0, -1, 0)
p4 = Obs(c4, GPSTime.readTimestamp("2018-01-01 10:01:50"))
self.trace1.addObs(p4)
self.trace1.addObs(p1)
# ---------------------------------------------------------------------
# Un escalier
self.trace2 = Track()
pm3 = Obs(ENUCoords(-2, -1),
GPSTime.readTimestamp('2020-01-01 09:59:44'))
self.trace2.addObs(pm3)
pm2 = Obs(ENUCoords(-1, -1),
GPSTime.readTimestamp('2020-01-01 09:59:48'))
self.trace2.addObs(pm2)
pm1 = Obs(ENUCoords(-1, 0),
GPSTime.readTimestamp('2020-01-01 09:59:55'))
self.trace2.addObs(pm1)
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace2.addObs(p1)
p2 = Obs(ENUCoords(0, 2), GPSTime.readTimestamp('2020-01-01 10:00:01'))
self.trace2.addObs(p2)
p3 = Obs(ENUCoords(1, 2), GPSTime.readTimestamp('2020-01-01 10:00:02'))
self.trace2.addObs(p3)
p4 = Obs(ENUCoords(1, 5), GPSTime.readTimestamp('2020-01-01 10:00:03'))
self.trace2.addObs(p4)
p5 = Obs(ENUCoords(2, 5), GPSTime.readTimestamp('2020-01-01 10:00:04'))
self.trace2.addObs(p5)
p6 = Obs(ENUCoords(2, 9), GPSTime.readTimestamp('2020-01-01 10:00:06'))
self.trace2.addObs(p6)
p7 = Obs(ENUCoords(3, 9), GPSTime.readTimestamp('2020-01-01 10:00:08'))
self.trace2.addObs(p7)
p8 = Obs(ENUCoords(3, 14),
GPSTime.readTimestamp('2020-01-01 10:00:10'))
self.trace2.addObs(p8)
p9 = Obs(ENUCoords(4, 14),
GPSTime.readTimestamp('2020-01-01 10:00:12'))
self.trace2.addObs(p9)
p10 = Obs(ENUCoords(4, 20),
GPSTime.readTimestamp('2020-01-01 10:00:15'))
self.trace2.addObs(p10)
# ---------------------------------------------------------------------
#
self.trace3 = Track()
p1 = Obs(ENUCoords(0, 0), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p1)
p2 = Obs(ENUCoords(1.5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p2)
p3 = Obs(ENUCoords(2, 2), GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p3)
p4 = Obs(ENUCoords(3.75, 0.6),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p4)
p5 = Obs(ENUCoords(5, 0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p5)
p6 = Obs(ENUCoords(3.55, -0.5),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p6)
p7 = Obs(ENUCoords(1.8, -1.2),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p7)
p8 = Obs(ENUCoords(1, -3),
GPSTime.readTimestamp('2020-01-01 10:00:00'))
self.trace3.addObs(p8)
def testCircleTrigo(self):
self.trace1.plot()
C1 = Geometrics.minCircle(self.trace1)
C1.plot()
self.assertLessEqual(abs(1 - C1.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(C1.center, ENUCoords)
self.assertLessEqual(abs(0 - C1.center.getX()), self.__epsilon,
"coord x du centre cercle")
self.assertLessEqual(abs(0 - C1.center.getY()), self.__epsilon,
"coord y du centre cercle")
C2 = Geometrics.fitCircle(self.trace1)
C2.plot()
self.assertLessEqual(abs(1 - C2.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(C2.center, ENUCoords)
self.assertLessEqual(abs(0 - C2.center.getX()), self.__epsilon,
"coord x du centre cercle")
self.assertLessEqual(abs(0 - C2.center.getY()), self.__epsilon,
"coord y du centre cercle")
plt.show()
def testCircles(self):
self.trace2.plot()
#plt.plot(track.getX(), track.getY(), 'b+')
circle1 = Geometrics.fitCircle(self.trace2)
self.assertIsInstance(circle1, Geometrics.Circle)
self.assertLessEqual(abs(28.363 - circle1.radius), self.__epsilon,
"Rayon du cercle")
self.assertIsInstance(circle1.center, ENUCoords)
self.assertLessEqual(abs(-25.09 - circle1.center.getX()),
self.__epsilon, "coord x du centre cercle")
self.assertLessEqual(abs(14.79 - circle1.center.getY()),
self.__epsilon, "coord y du centre cercle")
circle1.plot()
circle2 = Geometrics.minCircle(self.trace2)
self.assertIsInstance(circle2, Geometrics.Circle)
circle2.plot()
circle3 = Geometrics.minCircleMatrix(self.trace2)
self.assertEqual(circle3.size, 169)
# ??
plt.show()
def testDiameter(self):
D = Geometrics.diameter(self.trace1)
A = self.trace1.getObs(D[1])
B = self.trace1.getObs(D[2])
self.assertEqual(D[0], 2)
self.assertEqual(A.distanceTo(B), D[0])
self.assertEqual(A.position.getX(), 1)
self.assertEqual(A.position.getY(), 0)
self.assertEqual(B.position.getX(), -1)
self.assertEqual(B.position.getY(), 0)
D = Geometrics.diameter(self.trace2)
A = self.trace2.getObs(D[1])
B = self.trace2.getObs(D[2])
self.assertIsInstance(A, Obs)
self.assertIsInstance(B, Obs)
self.assertEqual(A.distanceTo(B), D[0])
def testConvexHull(self):
self.trace3.plot()
T = Geometrics.convexHull(self.trace3)
Geometrics.plotPolygon(T)
plt.show()
self.assertEqual(len(T), 2 * 5)
self.assertEqual(T[0], 0)
self.assertEqual(T[1], 0)
self.assertEqual(T[4], 5)
self.assertEqual(T[5], 0.5)
self.assertEqual(T[6], 1)
self.assertEqual(T[7], -3)
self.trace2.plot()
T = Geometrics.convexHull(self.trace2)
Geometrics.plotPolygon(T)
plt.show()
def testminimumBoundingRectangle(self):
self.trace3.plot()
R = Geometrics.minimumBoundingRectangle(self.trace3)
T = []
for coord in R[0]:
T.append(coord[0])
T.append(coord[1])
Geometrics.plotPolygon(T)
self.assertEqual(R[1], 16.5)
self.assertLessEqual(abs(3.104 - R[2]), self.__epsilon, "l")
self.assertLessEqual(abs(5.315 - R[3]), self.__epsilon, "L")