"""Class to manage filtering of GPS tracks"""
import numpy as np
from tracklib.core.Operator import Operator
from tracklib.algo.Dynamics import MODE_OBS_AND_STATES_AS_3D_POSITIONS
from tracklib.algo.Dynamics import DYN_MAT_2D_CST_SPEED
from tracklib.algo.Dynamics import HMM as dynamics_hmm
FILTER_LOW_PASS = 0 # Low-pass brick-wall filter
FILTER_HIGH_PASS = 1 # High-pass brick-wall filter
FILTER_TEMPORAL = 0
FILTER_SPATIAL = 1
FILTER_X = ["x"]
FILTER_Y = ["y"]
FILTER_Z = ["z"]
FILTER_XY = ["x", "y"]
FILTER_XZ = ["x", "z"]
FILTER_YZ = ["y", "z"]
FILTER_XYZ = ["x", "y", "z"]
KALMAN_FORWARD = 0
KALMAN_BACKWARD = 1
KALMAN_COMBINED = 2
# Parameter of generic
# -----------------------------------------
# Global variables for Markov filtering
# -----------------------------------------
sig = 0 #: TODO
res = 0 #: TODO
# -----------------------------------------
# --------------------------------------------------------------------------
# TODO: Gaussian process filtering
# --------------------------------------------------------------------------
[docs]def gaussianProcess(track):
"""TODO"""
return None
# --------------------------------------------------------------------------
# TODO: Wavelet filtering (Donoho and Johnstone)
# --------------------------------------------------------------------------
[docs]def waveletFiltering(track):
"""TODO"""
return None
# --------------------------------------------------------------------------
# TODO: Karhunen-Loeve filtering
# --------------------------------------------------------------------------
[docs]def KLBasis(TrackCollection):
"""TODO"""
return None
[docs]def KLFiltering(track):
"""TODO"""
return None
# --------------------------------------------------------------------------
# Filtering with Unscented Kalman filter based on speed regularization
# Inputs:
# - track: a track to filter
# - sigma: Positional standard deviation (in ground units)
# - speed: standard deviation of speed
# - speed_af: AF field containing speeds (optional)
# - mode : forward, backward or combined
# Important: tracks are assumed to be sampled at constant time frequency
# --------------------------------------------------------------------------
[docs]def __kalman(track, sigma, speed_std, speed_af=None, verbose=True):
"""TODO"""
track = track.copy()
dt = abs(track.frequency())
from tracklib.algo.Dynamics import Kalman as dynamics_kalman
# -----------------------------------------------------
# Mode speed recorded in AF field
# -----------------------------------------------------
if not (speed_af is None):
F = lambda x: DYN_MAT_2D_CST_SPEED(dt) @ x
H = lambda x: np.array(
[[x[0, 0]], [x[1, 0]], [(x[2, 0] ** 2 + x[3, 0] ** 2) ** 0.5]]
)
Q = np.eye(4, 4)
Q[2, 2] = 0
Q[3, 3] = 0
R = sigma ** 2 * np.eye(3, 3)
R[2, 2] = speed_std ** 2
X0 = np.array(
[[track[0].position.getX()], [track[0].position.getY()], [0], [0]]
)
P0 = sigma ** 2 * np.eye(4, 4)
UKF = dynamics_kalman(spreading=1)
UKF.setTransition(F, Q)
UKF.setObservation(H, R)
UKF.setInitState(X0, P0)
UKF.estimate(track, ["x", "y", speed_af], verbose=verbose)
# -----------------------------------------------------
# Mode prior information on speed based on std value
# -----------------------------------------------------
else:
F = lambda x: x # Transition model
H = lambda x: np.array([[x[0, 0]], [x[1, 0]]]) # Observation model
Q = (dt * speed_std) ** 2 * np.eye(2, 2) # Transition covariance
R = sigma ** 2 * np.eye(2, 2) # Observation covariance
p0 = track[0].position # First position
X0 = np.array([[p0.getX()], [p0.getY()]]) # Initial state
P0 = sigma ** 2 * np.eye(2, 2) # Initial covariance
UKF = dynamics_kalman(spreading=1)
UKF.setTransition(F, Q) # Dynamic model
UKF.setObservation(H, R) # Observation model
UKF.setInitState(X0, P0) # Initialization
UKF.estimate(track, ["x", "y"], verbose=verbose)
for i in range(len(track)):
track[i].position.setX(track.getObsAnalyticalFeature("kf_0", i))
track[i].position.setY(track.getObsAnalyticalFeature("kf_1", i))
return track
[docs]def Kalman(track, sigma, speed_std, speed_af=None, mode=KALMAN_FORWARD, verbose=True):
"""TODO"""
if mode == KALMAN_FORWARD:
return __kalman(track, sigma, speed_std, speed_af, verbose)
if mode == KALMAN_BACKWARD:
return __kalman(track.reverse(), sigma, speed_std, speed_af, verbose).reverse()
if mode == KALMAN_COMBINED:
track_fwd = Kalman(
track, sigma, speed_std, speed_af, mode=KALMAN_FORWARD, verbose=verbose
)
track_bwd = Kalman(
track, sigma, speed_std, speed_af, mode=KALMAN_BACKWARD, verbose=verbose
)
for k in range(len(track_fwd)):
x_fusion = (
0.5 * track_fwd[k].position.getX() + 0.5 * track_bwd[k].position.getX()
)
y_fusion = (
0.5 * track_fwd[k].position.getY() + 0.5 * track_bwd[k].position.getY()
)
track_fwd[k].position.setX(x_fusion)
track_fwd[k].position.setY(y_fusion)
return track_fwd
# --------------------------------------------------------------------------
# Filtering with Markov process based on speed regularization
# Inputs:
# - track: a track to filter
# - sigma: Positional standard deviation (in ground units)
# - speed: a function describing the speed distribution. It is a 3-valued
# function, where f(v,k,t) describes the (possibly non-normalized)
# probability value that track t is moving with speed v at epoch k. It
# may be (if necessary) computed with track analytical features or
# global external data such as DTM slope.
# - resolution: search grid resolution (in ground units). Note that the
# number N of states varies as the inverse of O(r^2), with r being the
# search grid resolution, while the time complexity of the overall
# algorithm is O(n^2). Therefore, the time required to compute a
# solution is proportional to the 4th power of (1/r). A resolution
# twice thinner implies 16 times longer computation. As thumb rule, the
# resoluton should be chosen around sigma/3, where sigma is the
# positional standrad deviation (in ground units). So, for a typical
# (standard positioning) GPS receiver with 3 m error, a search grid
# resolution of 1 m provides a thin modelization.
# --------------------------------------------------------------------------
[docs]def __Markov_S(track, i):
"""TODO"""
etats = []
N = int(2 * sig / res) + 1
for kx in range(-N, N + 1):
for ky in range(-N, N + 1):
p = track[i].position.copy()
p.translate(res * kx, res * ky)
etats.append(p)
return etats
[docs]def __Markov_Plog(pi, y, k, track):
"""TODO"""
return -((pi.distance2DTo(y) / 20) ** 2)
[docs]def MarkovRegularization(track, sigma, speed, resolution):
"""TODO"""
global res, sig
sig = sigma
res = resolution
model = dynamics_hmm()
model.setStates(__Markov_S)
model.setTransitionModel(speed)
model.setObservationModel(__Markov_Plog)
model.setLog(True)
model.estimate(track, obs=["x", "y", "z"], mode=MODE_OBS_AND_STATES_AS_3D_POSITIONS)
# --------------------------------------------------------------------------
# Generic method to filter a track in the frequency domain
# Mode :
# - FILTER_TEMPORAL (0)
# - FILTER_SPATIAL (1)
# Type :
# - FILTER_LOW_PASS (0) or "low" for low-pass filter
# - FILTER_HIGH_PASS (1) or "high" for high pass filter
# Cut-off frequency fc :
# - All frequencies above (resp. below) fc are filtered for low-pass
# Dimension dim : FILTER_X, FILTER_Y, FILTER_Z, FILTER_XY, FILTER_YZ,
# FILTER_XY or FILTER XYZ depending in the number of dimensions to filter
# (resp.) high-pass filter. May also be a list of analytical features names
# Spectral filtering is applied to a regularly sampled track. If track
# is temporally (resp. spatially) sampled, cut off frequencies are
# given in Hz or points/sec (resp. points/m or points/ground units).
# Note that pass-band and cut-band filters may be obtained by calling filter
# function twice sequentially with FILTER_LOW_PASS and FILTER_HIGH_PASS.
# --------------------------------------------------------------------------
[docs]def filter_freq(track, fc, mode=FILTER_TEMPORAL, type=FILTER_LOW_PASS, dim=FILTER_XYZ):
"""TODO"""
output = track.copy()
fs = output.frequency(mode)
for af in dim:
F = np.fft.fft(track.getAnalyticalFeature(af))
N = int(F.shape[0] / 2)
Nc = int(N * fc)
if type == FILTER_LOW_PASS:
F[Nc : F.shape[0] - Nc] = 0
else:
F[1:Nc] = 0
F[F.shape[0] - Nc : F.shape[0]] = 0
f = np.real(np.fft.ifft(F))
for i in range(len(output)):
output.setObsAnalyticalFeature(af, i, f[i])
return output
# --------------------------------------------------------------------------
# Generic method to filter a track in the sequence domain (time or spatial
# domain if the track is regularly sampled in time and/or space).
# Kernel, may be one of the following
# - a float number giving the half_width of rectangular window
# - an odd-sized float array giving the digital realization of a kernel
# - a Kernel object (GaussianKernel, ExponentialKernel...)
# Dimension dim : FILTER_X, FILTER_Y, FILTER_Z, FILTER_XY, FILTER_YZ,
# FILTER_XY or FILTER XYZ depending in the number of dimensions to filter.
# May also be a list of analytical features names
# Filtering is applied to a regularly sampled track. If track is temporally
# (resp. spatially) sampled, kernel dimensions are given as relative values
# to the temporal (resp. spatial) frequency.
# --------------------------------------------------------------------------
[docs]def filter_seq(track, kernel=1, dim=FILTER_XYZ):
#output = track.copy()
if isinstance(kernel, int):
kernel = [1]*kernel
if isinstance(kernel, list):
if len(kernel) == 1:
return track
for af in dim:
if af in ["x", "y", "z"]:
track.operate(Operator.FILTER, af, kernel, "temp")
if af == "x":
track.setXFromAnalyticalFeature("temp")
if af == "y":
track.setYFromAnalyticalFeature("temp")
if af == "z":
track.setZFromAnalyticalFeature("temp")
else:
track.operate(Operator.FILTER, af, kernel, af)
return track