From 3a72d8b28bde787d33a6f7704213919b508df358 Mon Sep 17 00:00:00 2001 From: Antoine COLLET Date: Mon, 27 Dec 2021 16:08:32 +0100 Subject: [PATCH 1/2] ENH; add support for numpy masked array for measurement used in update methods and functions. This allox the use of masked array in batch_filter instead of list with None. --- filterpy/hinfinity/hinfinity_filter.py | 2 +- filterpy/kalman/CubatureKalmanFilter.py | 2 +- filterpy/kalman/EKF.py | 2 +- filterpy/kalman/UKF.py | 2 +- filterpy/kalman/ensemble_kalman_filter.py | 2 +- filterpy/kalman/fading_memory.py | 2 +- filterpy/kalman/information_filter.py | 2 +- filterpy/kalman/kalman_filter.py | 17 +++-- filterpy/kalman/square_root.py | 2 +- filterpy/kalman/tests/ukf2.py | 88 +++++++++++------------ 10 files changed, 60 insertions(+), 61 deletions(-) diff --git a/filterpy/hinfinity/hinfinity_filter.py b/filterpy/hinfinity/hinfinity_filter.py index 9ed320ea..4eb87e79 100644 --- a/filterpy/hinfinity/hinfinity_filter.py +++ b/filterpy/hinfinity/hinfinity_filter.py @@ -101,7 +101,7 @@ def update(self, z): measurement for this update. """ - if z is None: + if z is None or z is np.ma.masked: return # rename for readability and a tiny extra bit of speed diff --git a/filterpy/kalman/CubatureKalmanFilter.py b/filterpy/kalman/CubatureKalmanFilter.py index bae2a1a5..6692eb15 100644 --- a/filterpy/kalman/CubatureKalmanFilter.py +++ b/filterpy/kalman/CubatureKalmanFilter.py @@ -345,7 +345,7 @@ def update(self, z, R=None, hx_args=()): variable. """ - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() diff --git a/filterpy/kalman/EKF.py b/filterpy/kalman/EKF.py index 5689d848..b89420f5 100644 --- a/filterpy/kalman/EKF.py +++ b/filterpy/kalman/EKF.py @@ -294,7 +294,7 @@ def update(self, z, HJacobian, Hx, R=None, args=(), hx_args=(), example, if they are angles) """ - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() diff --git a/filterpy/kalman/UKF.py b/filterpy/kalman/UKF.py index f2f2c77e..00d3e68e 100644 --- a/filterpy/kalman/UKF.py +++ b/filterpy/kalman/UKF.py @@ -439,7 +439,7 @@ def update(self, z, R=None, UT=None, hx=None, **hx_args): arguments to be passed into h(x) after x -> h(x, **hx_args) """ - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self._dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() diff --git a/filterpy/kalman/ensemble_kalman_filter.py b/filterpy/kalman/ensemble_kalman_filter.py index 83b9bb56..ec6bb78a 100644 --- a/filterpy/kalman/ensemble_kalman_filter.py +++ b/filterpy/kalman/ensemble_kalman_filter.py @@ -231,7 +231,7 @@ def update(self, z, R=None): one call, otherwise self.R will be used. """ - if z is None: + if z is None or z is np.ma.masked: self.z = array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() diff --git a/filterpy/kalman/fading_memory.py b/filterpy/kalman/fading_memory.py index 36c9dcc9..2ea4f474 100644 --- a/filterpy/kalman/fading_memory.py +++ b/filterpy/kalman/fading_memory.py @@ -208,7 +208,7 @@ def update(self, z, R=None): one call, otherwise self.R will be used. """ - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() diff --git a/filterpy/kalman/information_filter.py b/filterpy/kalman/information_filter.py index 46724d3e..771d46ad 100644 --- a/filterpy/kalman/information_filter.py +++ b/filterpy/kalman/information_filter.py @@ -191,7 +191,7 @@ def update(self, z, R_inv=None): one call, otherwise self.R will be used. """ - if z is None: + if z is None or z is np.ma.masked: self.z = None self.x_post = self.x.copy() self.P_inv_post = self.P_inv.copy() diff --git a/filterpy/kalman/kalman_filter.py b/filterpy/kalman/kalman_filter.py index 74dddde6..1941b0ae 100644 --- a/filterpy/kalman/kalman_filter.py +++ b/filterpy/kalman/kalman_filter.py @@ -200,7 +200,7 @@ class KalmanFilter(object): f.H = np.array([[1., 0.]]) - Define the state's covariance matrix P. + Define the state's covariance matrix P. .. code:: @@ -512,7 +512,7 @@ def update(self, z, R=None, H=None): self._likelihood = None self._mahalanobis = None - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() @@ -641,7 +641,7 @@ def update_steadystate(self, z): self._likelihood = None self._mahalanobis = None - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() @@ -702,7 +702,7 @@ def update_correlated(self, z, R=None, H=None): self._likelihood = None self._mahalanobis = None - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() @@ -1070,7 +1070,7 @@ def get_update(self, z=None): State vector and covariance array of the update. """ - if z is None: + if z is None or z is np.ma.masked: return self.x, self.P z = reshape_z(z, self.dim_z, self.x.ndim) @@ -1183,7 +1183,7 @@ def log_likelihood_of(self, z): after a call to update(). Calling after predict() will yield an incorrect result.""" - if z is None: + if z is None or z is np.ma.masked: return log(sys.float_info.min) return logpdf(z, dot(self.H, self.x), self.S) @@ -1387,7 +1387,7 @@ def update(x, P, z, R, H=None, return_all=False): #pylint: disable=bare-except - if z is None: + if z is None or z is np.ma.masked: if return_all: return x, P, None, None, None, None return x, P @@ -1476,8 +1476,7 @@ def update_steadystate(x, z, K, H=None): >>> update_steadystate(x, P, z, H) """ - - if z is None: + if z is None or z is np.ma.masked: return x if H is None: diff --git a/filterpy/kalman/square_root.py b/filterpy/kalman/square_root.py index 6dd445c7..8ccfe893 100644 --- a/filterpy/kalman/square_root.py +++ b/filterpy/kalman/square_root.py @@ -188,7 +188,7 @@ def update(self, z, R2=None): be used. """ - if z is None: + if z is None or z is np.ma.masked: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self._P1_2_post = np.copy(self._P1_2) diff --git a/filterpy/kalman/tests/ukf2.py b/filterpy/kalman/tests/ukf2.py index 989bf5bc..ae5bf2d7 100644 --- a/filterpy/kalman/tests/ukf2.py +++ b/filterpy/kalman/tests/ukf2.py @@ -335,7 +335,7 @@ def update(self, z, R=None, UT=None, hx_args=()): variable. """ - if z is None: + if z is None or z is np.ma.masked: return if not isinstance(hx_args, tuple): @@ -543,34 +543,34 @@ def rts_smoother(self, Xs, Ps, Qs=None, dt=None): return (xs, ps, Ks) - + class MerweScaledSigmaPoints2(object): - + def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): """ Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1]. It parametizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications. - + Unless you know better, this should be your default choice. - + Parameters ---------- - + n : int Dimensionality of the state. 2n+1 weights will be generated. - + alpha : float Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) according to [3]. - + beta : float Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3]. - + kappa : float, default=0.0 Secondary scaling parameter usually set to 0 according to [4], or to 3-n according to [5]. - + sqrt_method : function(ndarray), default=scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its @@ -581,26 +581,26 @@ def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. - + If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. - + subtract : callable (x, y), optional Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. - + References ---------- - + .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) - + """ - + self.n = n self.alpha = alpha self.beta = beta @@ -609,94 +609,94 @@ def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): self.sqrt = cholesky else: self.sqrt = sqrt_method - + if subtract is None: self.subtract= np.subtract else: self.subtract = subtract - - + + def num_sigmas(self): """ Number of sigma points for each variable in the state x""" return 2*self.n + 1 - - + + def sigma_points(self, x, P): """ Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights. - + Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I - + Parameters ---------- - + X An array-like object of the means of length n Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2]) - + P : scalar, or np.array Covariance of the filter. If scalar, is treated as eye(n)*P. - + Returns ------- - + sigmas : np.array, of size (n, 2n+1) Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space. - + Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n} """ - - + + assert x.ndim == 2 and x.shape[1], "x must be a column vector" - + n = self.n - + if np.isscalar(P): P = np.eye(n)*P else: P = np.asarray(P) - + lambda_ = self.alpha**2 * (n + self.kappa) - n U = self.sqrt((lambda_ + n)*P) - + sigmas = np.zeros((n, 2*n+1)) x0 = x[:, 0] sigmas[:,0] = x0 for k in range(n): sigmas[:, k+1] = self.subtract(x0, -U[k]) sigmas[:, n+k+1] = self.subtract(x0, U[k]) - + return sigmas - - + + def weights(self): """ Computes the weights for the scaled unscented Kalman filter. - + Returns ------- - + Wm : ndarray[2n+1] weights for mean - + Wc : ndarray[2n+1] weights for the covariances """ - + n = self.n lambda_ = self.alpha**2 * (n +self.kappa) - n - + c = .5 / (n + lambda_) Wc = np.full(2*n + 1, c) Wm = np.full(2*n + 1, c) Wc[0] = lambda_ / (n + lambda_) + (1 - self.alpha**2 + self.beta) Wm[0] = lambda_ / (n + lambda_) - + return Wm, Wc - + def unscented_transform2(sigmas, Wm, Wc, noise_cov=None, mean_fn=np.dot, residual_fn=None): """ Computes unscented transform of a set of sigma points and weights. From b8b7ef32657f678db8690b23a0cb5ae322deed1c Mon Sep 17 00:00:00 2001 From: Antoine COLLET Date: Mon, 27 Dec 2021 16:21:02 +0100 Subject: [PATCH 2/2] TEST: add support for numpy masked array for measurement used in update methods and functions. This allox the use of masked array in batch_filter instead of list with None. --- filterpy/kalman/tests/test_kf.py | 5 +++++ filterpy/kalman/tests/test_ukf.py | 12 +++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/filterpy/kalman/tests/test_kf.py b/filterpy/kalman/tests/test_kf.py index e58e0909..daa49c4b 100644 --- a/filterpy/kalman/tests/test_kf.py +++ b/filterpy/kalman/tests/test_kf.py @@ -268,10 +268,15 @@ def test_batch_filter(): f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty + # Test with None for missing measurments zs = [None, 1., 2.] m, c, _, _ = f.batch_filter(zs, update_first=False) m, c, _, _ = f.batch_filter(zs, update_first=True) + # Test with masked array for missing measurments + zs = np.ma.masked_invalid([np.nan, 1., 2.]) + m, c, _, _ = f.batch_filter(zs, update_first=False) + m, c, _, _ = f.batch_filter(zs, update_first=True) def test_univariate(): f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1) diff --git a/filterpy/kalman/tests/test_ukf.py b/filterpy/kalman/tests/test_ukf.py index b2f0d45d..f0c8f0d0 100644 --- a/filterpy/kalman/tests/test_ukf.py +++ b/filterpy/kalman/tests/test_ukf.py @@ -430,7 +430,11 @@ def hx(x): def test_batch_missing_data(): - """ batch filter should accept missing data with None in the measurements """ + """ + Batch filter should accept missing data with None in the measurements + + Alternatively, a masked array can be used. + """ def fx(x, dt): F = np.array([[1, dt, 0, 0], @@ -456,11 +460,17 @@ def hx(x): z = np.array([i + randn()*0.1, i + randn()*0.1]) zs.append(z) + # Test with None for missing measurments zs[2] = None Rs = [1]*len(zs) Rs[2] = None Ms, Ps = kf.batch_filter(zs) + # Test with masked array for missing measurments + zs[2] = np.nan + zs = np.ma.masked_invalid([np.nan, 1., 2.]) + Ms, Ps = kf.batch_filter(zs) + def test_rts(): def fx(x, dt):