From 8e411b384d59f629a089fafb8affb89728298bec Mon Sep 17 00:00:00 2001 From: Per A Brodtkorb Date: Sat, 27 Feb 2016 00:08:28 +0100 Subject: [PATCH] updated Kalman and HampelFilter --- wafo/sg_filter.py | 99 +++++++++++++++++++++++++++++------------------ 1 file changed, 62 insertions(+), 37 deletions(-) diff --git a/wafo/sg_filter.py b/wafo/sg_filter.py index eca2050..8041725 100644 --- a/wafo/sg_filter.py +++ b/wafo/sg_filter.py @@ -572,7 +572,7 @@ def smoothn(data, s=None, weight=None, robust=False, z0=None, tolz=1e-3, def gcv(p, aow, Lambda, DCTy, y, Wtot, IsFinite, nof, noe): # Search the smoothing parameter s that minimizes the GCV score - s = 10 ** p + s = 10.0 ** p Gamma = 1.0 / (1 + s * Lambda ** 2) # RSS = Residual sum-of-squares if aow > 0.9: # aow = 1 means that all of the data are equally weighted @@ -589,7 +589,6 @@ def gcv(p, aow, Lambda, DCTy, y, Wtot, IsFinite, nof, noe): return GCVscore -# Robust weights def RobustWeights(r, I, h, wstr): # weights for robust smoothing. MAD = np.median(abs(r[I] - np.median(r[I]))) # median absolute deviation @@ -882,7 +881,7 @@ class Kalman(object): self.x = x # Initial state estimate. self.P = P # Initial covariance estimate. self.A = A # State transition matrix. - self.B = B # Control matrix. + self.B = B # Control matrix. self.Q = Q # Estimated error in process. self.H = H # Observation matrix. self.reset() @@ -890,38 +889,55 @@ class Kalman(object): def reset(self): self._filter = self._filter_first - def _filter_first(self, z, u): - - self._filter = self._filter_main - - auto_init = self.x is None - if auto_init: - n = np.size(z) - else: - n = np.size(self.x) + def _set_A(self, n): if self.A is None: self.A = np.eye(n) self.A = np.atleast_2d(self.A) + + def _set_Q(self, n): if self.Q is None: self.Q = np.zeros((n, n)) self.Q = np.atleast_2d(self.Q) + + def _set_H(self, n): if self.H is None: self.H = np.eye(n) self.H = np.atleast_2d(self.H) + + def _set_P(self, HI): + if self.P is None: + self.P = np.dot(np.dot(HI, self.R), HI.T) + self.P = np.atleast_2d(self.P) + + def _init_first(self, n): + self._set_A(n) + self._set_Q(n) + self._set_H(n) try: HI = np.linalg.inv(self.H) except: HI = np.eye(n) - if self.P is None: - self.P = np.dot(np.dot(HI, self.R), HI.T) + self._set_P(HI) + return HI - self.P = np.atleast_2d(self.P) - if auto_init: - # initialize state estimate from first observation - self.x = np.dot(HI, z) + def _first_state(self, z): + n = np.size(z) + HI = self._init_first(n) + # initialize state estimate from first observation + x = np.dot(HI, z) + return x + + def _filter_first(self, z, u): + + self._filter = self._filter_main + + if self.x is None: + self.x = self._first_state(z) return self.x - else: - return self._filter_main(z, u) + + n = np.size(self.x) + self._init_first(n) + return self._filter_main(z, u) def _predict_state(self, x, u): return np.dot(self.A, x) + np.dot(self.B, u) @@ -1043,12 +1059,12 @@ def lti_disc(F, L=None, Q=None, dt=1): def test_kalman_sine(): """Kalman Filter demonstration with sine signal.""" - sd = 1. + sd = 0.5 dt = 0.1 w = 1 T = np.arange(0, 30 + dt / 2, dt) n = len(T) - X = np.sin(w * T) + X = 3*np.sin(w * T) Y = X + sd * np.random.randn(n) ''' Initialize KF to values @@ -1091,7 +1107,7 @@ def test_kalman_sine(): _ht = plt.plot(truth, 'g-', label='true voltage') plt.legend() plt.title('Automobile Voltimeter Example') - plt.show() + plt.show('hold') # for k in range(m): # [M,P] = kf_predict(M,P,A,Q); @@ -1274,6 +1290,22 @@ class HampelFilter(object): ADX = dx * np.ones(Y.shape) return Y0, S0, ADX + def _fixed(self, Y, X, dx): + localwindow = self.localwindow + Y0, S0, ADX = self._init(Y, dx) + for i in range(len(Y)): + Y0[i], S0[i] = localwindow(X, Y, dx, i) + return Y0, S0, ADX + + def _filter(self, Y, X, dx): + if len(X) <= 1: + Y0, S0, ADX = self._init(Y, dx) + elif self.adaptive is None: + Y0, S0, ADX = self._fixed(Y, X, dx) + else: + Y0, S0, ADX = self._adaptive(Y, X, dx) # 'adaptive' + return Y0, S0, ADX + def __call__(self, y, x=None): Y = np.atleast_1d(y).ravel() if x is None: @@ -1283,21 +1315,14 @@ class HampelFilter(object): dx = 3 * np.median(np.diff(X)) if self.dx is None else self.dx self._check(dx) - if len(X) > 1: - if self.adaptive is None: - localwindow = self.localwindow - Y0, S0, ADX = self._init(Y, dx) - for i in range(len(Y)): - Y0[i], S0[i] = localwindow(X, Y, dx, i) - else: # 'adaptive' - Y0, S0, ADX = self._adaptive(Y, X, dx) + Y0, S0, ADX = self._filter(Y, X, dx) YY = Y.copy() T = self.t # Prepare Output self.UB = Y0 + T * S0 self.LB = Y0 - T * S0 outliers = np.abs(Y - Y0) > T * S0 # possible outliers - YY[outliers] = Y0[outliers] + np.putmask(YY, outliers, Y0) # YY[outliers] = Y0[outliers] self.outliers = outliers self.num_outliers = outliers.sum() self.ADX = ADX @@ -1308,7 +1333,7 @@ class HampelFilter(object): return YY -def test_hampel(): +def demo_hampel(): randint = np.random.randint Y = 5000 + np.random.randn(1000) outliers = randint(0, 1000, size=(10,)) @@ -1450,12 +1475,12 @@ def test_docstrings(): doctest.testmod(optionflags=doctest.NORMALIZE_WHITESPACE) if __name__ == '__main__': - test_docstrings() - # test_kalman_sine() + # test_docstrings() + test_kalman_sine() # test_tide_filter() - # test_hampel() + # demo_hampel() # test_kalman() # test_smooth() # test_hodrick_cardioid() - test_smoothn_1d() + # test_smoothn_1d() # test_smoothn_cardioid()