def findStopsLocal(track, speed=1, duration=10):
track = track.copy()
stops = Track()
# track.segmentation("speed", "#mark", speed)
track.operate(Operator.Operator.DIFFERENTIATOR, "#mark")
track.operate(Operator.Operator.RECTIFIER, "#mark")
TRACES = split(track, "#mark")
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
TMP_SIGMA_X = []
TMP_SIGMA_Y = []
TMP_SIGMA_Z = []
for i in range(0, len(TRACES), 2):
if TRACES[i].duration() < duration:
continue
stops.addObs(
Obs(TRACES[i].getCentroid().copy(),
TRACES[i].getFirstObs().timestamp.copy()))
TMP_SIGMA_X.append(TRACES[i].operate(Operator.Operator.AVERAGER, "x"))
TMP_SIGMA_Y.append(TRACES[i].operate(Operator.Operator.AVERAGER, "y"))
TMP_SIGMA_Z.append(TRACES[i].operate(Operator.Operator.AVERAGER, "z"))
TMP_SIGMA_X.append(TRACES[i].operate(Operator.Operator.STDDEV, "x"))
TMP_SIGMA_Y.append(TRACES[i].operate(Operator.Operator.STDDEV, "y"))
TMP_SIGMA_Z.append(TRACES[i].operate(Operator.Operator.STDDEV, "z"))
TMP_NBPOINTS.append(TRACES[i].size())
TMP_DURATION.append(TRACES[i].duration())
if stops.size() == 0:
return stops
# stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
return stops
def findStopsGlobalForRTK(track,
std_max=2e-2,
duration=5,
downsampling=1,
verbose=True):
"""Find stop points in a track based on maximal size of a stop and minimal
time duration
Two parameters:
- Maximal size of a stop (as the standard deviation per axis, in ground units)
- Minimal time duration (in seconds)
Use downsampling parameter > 1 to speed up the process.
Default is set for precise RTK GNSS survey (2 cm for 5 sec)
"""
# If down-sampling is required
if downsampling > 1:
track = track.copy()
track **= track.size() / downsampling
# ---------------------------------------------------------------------------
# Computes cost matrix as :
# Cij = 0 if sqrt(0.33*(std_x^2 + std_y^2 + std_Z^2)) > std_max
# Cij = 0 if time duration between pi and p-1 is < duration
# Cij = (j-i)**2 = square of the number of points of segment otherwise
# ---------------------------------------------------------------------------
C = np.zeros((track.size(), track.size()))
RANGE = range(track.size() - 2)
if verbose:
print("Minimal enclosing circles computation:")
RANGE = progressbar.progressbar(RANGE)
for i in RANGE:
for j in range(i + 1, track.size() - 1):
if track[i].distanceTo(track[j - 1]) > 3 * std_max:
C[i, j] = 0
break
if track[j - 1].timestamp - track[i].timestamp <= duration:
C[i, j] = 0
continue
portion = track.extract(i, j - 1)
varx = portion.operate(Operator.Operator.VARIANCE, "x")
vary = portion.operate(Operator.Operator.VARIANCE, "y")
varz = portion.operate(Operator.Operator.VARIANCE, "z")
C[i, j] = math.sqrt(varx + vary + varz)
C[i, j] = (C[i, j] < std_max) * (j - i)**2
C = C + np.transpose(C)
# ---------------------------------------------------------------------------
# Computes optimal partition with dynamic programing
# ---------------------------------------------------------------------------
segmentation = optimalPartition(C, MODE_SEGMENTATION_MAXIMIZE, verbose)
stops = Track()
TMP_RADIUS = []
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_IDSTART = []
TMP_IDEND = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
for i in range(len(segmentation) - 1):
portion = track.extract(segmentation[i], segmentation[i + 1] - 1)
radius = C[segmentation[i], segmentation[i + 1]]
if radius == 0:
continue
xm = portion.operate(Operator.Operator.AVERAGER, "x")
ym = portion.operate(Operator.Operator.AVERAGER, "y")
zm = portion.operate(Operator.Operator.AVERAGER, "z")
xv = portion.operate(Operator.Operator.VARIANCE, "x")
yv = portion.operate(Operator.Operator.VARIANCE, "y")
zv = portion.operate(Operator.Operator.VARIANCE, "z")
pt = portion[0].position.copy()
pt.setX(xm)
pt.setY(ym)
pt.setZ(zm)
stops.addObs(Obs(pt, portion[0].timestamp))
TMP_RADIUS.append(math.sqrt(xv + yv + zv))
TMP_MEAN_X.append(xm)
TMP_MEAN_Y.append(ym)
TMP_MEAN_Z.append(zm)
TMP_STD_X.append(xv**0.5)
TMP_STD_Y.append(yv**0.5)
TMP_STD_Z.append(zv**0.5)
TMP_IDSTART.append(segmentation[i] * downsampling)
TMP_IDEND.append((segmentation[i + 1] - 1) * downsampling)
TMP_NBPOINTS.append(segmentation[i + 1] - segmentation[i])
TMP_DURATION.append(portion.duration())
if stops.size() == 0:
return stops
stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("id_ini", TMP_IDSTART)
stops.createAnalyticalFeature("id_end", TMP_IDEND)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
stops.base = track.base
return stops
def findStopsGlobal(track,
diameter=20,
duration=60,
downsampling=1,
verbose=True):
"""Find stop points in a track based on two parameters:
Maximal size of a stop (as the diameter of enclosing circle,
in ground units) and minimal time duration (in seconds)
Use downsampling parameter > 1 to speed up the process"""
# If down-sampling is required
if downsampling > 1:
track = track.copy()
track **= track.size() / downsampling
# ---------------------------------------------------------------------------
# Computes cost matrix as :
# Cij = 0 if size of enclosing circle of pi, pi+1, ... pj-1 is > diameter
# Cij = 0 if time duration between pi and p-1 is < duration
# Cij = (j-i)**2 = square of the number of points of segment otherwise
# ---------------------------------------------------------------------------
C = np.zeros((track.size(), track.size()))
RANGE = range(track.size() - 2)
if verbose:
print("Minimal enclosing circles computation:")
RANGE = progressbar.progressbar(RANGE)
for i in RANGE:
for j in range(i + 1, track.size() - 1):
if track[i].distance2DTo(track[j - 1]) > diameter:
C[i, j] = 0
break
if track[j - 1].timestamp - track[i].timestamp <= duration:
C[i, j] = 0
continue
C[i, j] = 2 * minCircle(track.extract(i, j - 1))[1]
C[i, j] = (C[i, j] < diameter) * (j - i)**2
C = C + np.transpose(C)
# ---------------------------------------------------------------------------
# Computes optimal partition with dynamic programing
# ---------------------------------------------------------------------------
segmentation = optimalPartition(C, MODE_SEGMENTATION_MAXIMIZE, verbose)
stops = Track()
TMP_RADIUS = []
TMP_MEAN_X = []
TMP_MEAN_Y = []
TMP_MEAN_Z = []
TMP_IDSTART = []
TMP_IDEND = []
TMP_STD_X = []
TMP_STD_Y = []
TMP_STD_Z = []
TMP_DURATION = []
TMP_NBPOINTS = []
for i in range(len(segmentation) - 1):
portion = track.extract(segmentation[i], segmentation[i + 1] - 1)
C = minCircle(portion)
if (C[1] > diameter / 2) or (portion.duration() < duration):
continue
stops.addObs(Obs(C[0], portion.getFirstObs().timestamp))
TMP_RADIUS.append(C[1])
TMP_MEAN_X.append(portion.operate(Operator.Operator.AVERAGER, "x"))
TMP_MEAN_Y.append(portion.operate(Operator.Operator.AVERAGER, "y"))
TMP_MEAN_Z.append(portion.operate(Operator.Operator.AVERAGER, "z"))
TMP_STD_X.append(portion.operate(Operator.Operator.STDDEV, "x"))
TMP_STD_Y.append(portion.operate(Operator.Operator.STDDEV, "y"))
TMP_STD_Z.append(portion.operate(Operator.Operator.STDDEV, "z"))
TMP_IDSTART.append(segmentation[i] * downsampling)
TMP_IDEND.append((segmentation[i + 1] - 1) * downsampling)
TMP_NBPOINTS.append(segmentation[i + 1] - segmentation[i])
TMP_DURATION.append(portion.duration())
if stops.size() == 0:
return stops
stops.createAnalyticalFeature("radius", TMP_RADIUS)
stops.createAnalyticalFeature("mean_x", TMP_MEAN_X)
stops.createAnalyticalFeature("mean_y", TMP_MEAN_Y)
stops.createAnalyticalFeature("mean_z", TMP_MEAN_Z)
stops.createAnalyticalFeature("id_ini", TMP_IDSTART)
stops.createAnalyticalFeature("id_end", TMP_IDEND)
stops.createAnalyticalFeature("sigma_x", TMP_STD_X)
stops.createAnalyticalFeature("sigma_y", TMP_STD_Y)
stops.createAnalyticalFeature("sigma_z", TMP_STD_Z)
stops.createAnalyticalFeature("duration", TMP_DURATION)
stops.createAnalyticalFeature("nb_points", TMP_NBPOINTS)
stops.operate(Operator.Operator.QUAD_ADDER, "sigma_x", "sigma_y", "rmse")
stops.base = track.base
return stops
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()
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")