From de02cd5fd7780094396e6b53e4294b08a2259b4d Mon Sep 17 00:00:00 2001 From: John Hiles Date: Tue, 20 Feb 2024 14:53:55 -0500 Subject: [PATCH 01/51] Added Stochastic Integration Filter and associated functions. --- stonesoup/functions/__init__.py | 119 +++++++++++++++++++++ stonesoup/predictor/kalman.py | 179 +++++++++++++++++++++++++++++++- stonesoup/smoother/kalman.py | 104 ++++++++++++++++++- stonesoup/updater/kalman.py | 149 +++++++++++++++++++++++++- 4 files changed, 543 insertions(+), 8 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 991e9df6c..791f829d7 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -761,3 +761,122 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0): a, b = fun(state_x, t) state_x.state_vector = state_x.state_vector + a*delta_t + b@delta_w return state_x.state_vector + +def RandOrthMat(n,tol=1e-6): + """Random orthogonal real matrix + + M = RANDORTHMAT(n) + generates a random n x n orthogonal real matrix. + M = RANDORTHMAT(n,tol) + explicitly specifies a thresh value that measures linear dependence + of a newly formed column with the existing columns. Defaults to 1e-6. + + In this version the generated matrix distribution *is* uniform over the manifold + O(n) w.r.t. the induced R^(n^2) Lebesgue measure, at a slight computational + overhead (randn + normalization, as opposed to rand ). + + (c) Ofek Shilon , 2006. + + Parameters + ========== + n integer + matrix size + + Returns + ======= + numpy.ndarray n x n + Random orthogonal real matrix + """ + + M = np.zeros((n,n)) # prealloc + +# # Gram-schmidt on random column vectors + vi = np.random.rand(n,1) +# # the n-dimensional normal distribution has spherical symmetry, which implies +# # that after normalization the drawn vectors would be uniformly distributed on the +# # n-dimensional unit sphere. + M[:,0] = (vi*1/(np.linalg.norm(vi))).T + + for i in range(1,n): + nrm = 0 + while nrm self.Eps), ])]): + N = N+1 + # -- cubature points and weights computation (for standard normal PDF) + [SCRSigmaPoints,w] = stochasticCubatureRulePoints(enx,self.SIorder) + # -- points transformation for given filtering mean and covariance + # matrix + xpoints = Sp@SCRSigmaPoints + npm.repmat(prior.mean,1,np.size(SCRSigmaPoints,1)) + # -- points transformation via dynamics (deterministic part) + # Put these points into s State object list + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(prior) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + + fpoints = StateVectors([ + transition_and_control_function(sigma_points_state) for sigma_points_state in sigma_points_states]) + # -- stochastic integration rule for predictive state mean and covariance + # matrix + # SumRx = fpoints@w.reshape(np.size(w),1) + SumRx = np.average(fpoints, axis=1, weights=w); + + # --- update mean Ix + Dx = (SumRx-Ix)/N + Ix = Ix+Dx + Vx = (N-2)*Vx/N+Dx**2 + + xp = Ix + + + N = 0 + # - SIR recursion for state predictive moments computation + # -- until either required number of iterations is reached or threshold is reached + while N self.Eps)])]): + N = N+1 + # -- cubature points and weights computation (for standard normal PDF) + [SCRSigmaPoints,w] = stochasticCubatureRulePoints(enx,self.SIorder) + # -- points transformation for given filtering mean and covariance + # matrix + xpoints = Sp@SCRSigmaPoints + npm.repmat(prior.mean,1,np.size(SCRSigmaPoints,1)) + # -- points transformation via dynamics (deterministic part) + # Put these points into s State object list + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(prior) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + + fpoints = StateVectors([ + transition_and_control_function(sigma_points_state) for sigma_points_state in sigma_points_states]) + # -- stochastic integration rule for predictive state mean and covariance + # matrix + + fpoints_diff = fpoints - xp + SumRPx = fpoints_diff@(np.diag(w))@(fpoints_diff.T); + # SumRPx=np.multiply(pom,fpoints)@fpoints.T + + # --- update covariance matrix IPx + DPx = (SumRPx-IPx)/N + IPx = IPx+DPx + VPx = (N-2)*VPx/N+DPx**2 + + + Q = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) + + + Pp = IPx + Pp = Pp + Q + np.diag(Vx.ravel()); + Pp = (Pp+Pp.T)/2 + + Pp = Pp.astype(np.float64) + + + # and return a Gaussian state based on these parameters + return Prediction.from_state(prior, xp.view(StateVector), Pp.view(CovarianceMatrix), timestamp=timestamp, + transition_model=self.transition_model, prior=prior) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 141f54d1a..b9a2b386a 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -11,8 +11,8 @@ from ..types.update import GaussianStateUpdate from ..models.transition.base import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel -from ..functions import gauss2sigma, unscented_transform - +from ..functions import gauss2sigma, unscented_transform, stochasticCubatureRulePoints +from ..types.array import StateVector, StateVectors class KalmanSmoother(Smoother): r""" @@ -304,4 +304,102 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): sigma_point_states, mean_weights, covar_weights, transition_function) - return cross_covar @ np.linalg.inv(prediction.covar) + return cross_covar @ np.linalg.inv(prediction.covar) + +class SIFKalmanSmoother(KalmanSmoother): + r"""The stochastic integration version of the Kalman filter. As with the parent version of the Kalman + smoother, the mean and covariance of the prediction are retrieved from the track. The + stochastic integration is used to calculate the smoothing gain. + + """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") + + Nmax: float = Property( + default=10, + doc="maximal number of iterations of SIR") + Nmin: float = Property( + default=5, + doc="minimal number of iterations of stochastic integration rule (SIR)") + Eps: float = Property( + default=5e-3, + doc="allowed threshold for integration error") + SIorder: float = Property( + default=3, + doc="order of SIR (orders 1, 3, 5 are currently supported)") + + def _smooth_gain(self, state, prediction, time_interval, **kwargs): + """Calculate the smoothing gain + + Parameters + ---------- + state : :class:`~.State` + The input state + prediction : :class:`~.GaussianStatePrediction` + The prediction from the subsequent timestep + + Returns + ------- + : Matrix + The smoothing gain + + """ + + # - initialisation + predMean = prediction.mean + filtMean = state.mean + filtVar = state.covar + + + # This ensures that the time interval is correctly applied. + transition_function = partial( + self._transition_model(state).function, + time_interval=time_interval) + + + nx = predMean.shape[0] + + efMean = filtMean.copy() + efVar = filtVar.copy() + Sf = np.linalg.cholesky(efVar) + IPxx = np.zeros((nx, nx)) + VPxx = np.zeros((nx, nx)) + N = 0 # number of iterations + + # - SIR recursion for measurement predictive moments computation + # -- until either required number of iterations is reached or threshold is reached + while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): + N += 1 + + # -- cubature points and weights computation (for standard normal PDF) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) + + # -- points transformation for given filtering mean and covariance matrix + # xpoints = np.dot(Sf, SCRSigmaPoints) + efMean[:, np.newaxis] + xpoints = Sf@SCRSigmaPoints + np.matlib.repmat(efMean,1,np.size(SCRSigmaPoints,1)) + # -- points transformation via measurement equation (deterministic part) + # fpoints = ffunct(xpoints, wMean, Time) + + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(prediction) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + fpoints = StateVectors([ + transition_function(sigma_points_state) for sigma_points_state in sigma_points_states]) + + # -- stochastic integration rule for predictive measurement mean and covariance matrix + fpoints_diff = fpoints - predMean + xpoints_diff = xpoints - filtMean + SumRPxx = xpoints_diff@(fpoints_diff*np.matlib.repmat(w,nx,1)).T + + + # --- update cross-covariance matrix IPxx + DPxx = (SumRPxx - IPxx) / N + IPxx += DPxx + VPxx = (N - 2) * VPxx / N + DPxx**2 + + # - measurement predictive moments + Pxxps = IPxx + + + return Pxxps @ np.linalg.inv(prediction.covar) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index e4644b571..d61c4c7a4 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1,21 +1,21 @@ import warnings +import copy import numpy as np import scipy.linalg as la from functools import lru_cache from ..base import Property from .base import Updater -from ..types.array import CovarianceMatrix, StateVector +from ..types.array import CovarianceMatrix, StateVector, StateVectors from ..types.prediction import MeasurementPrediction from ..types.update import Update from ..models.base import LinearModel from ..models.measurement.linear import LinearGaussian from ..models.measurement import MeasurementModel -from ..functions import gauss2sigma, unscented_transform +from ..functions import gauss2sigma, unscented_transform, stochasticCubatureRulePoints from ..measures import Measure, Euclidean - class KalmanUpdater(Updater): r"""A class which embodies Kalman-type updaters; also a class which performs measurement update step as in the standard Kalman filter. @@ -796,3 +796,146 @@ def _posterior_covariance(self, hypothesis): post_cov[np.ix_(self.consider, ~self.consider)] -= khp.T return post_cov.view(CovarianceMatrix), kalman_gain + +class SIFKalmanUpdater(KalmanUpdater): + """The SIF version of the Kalman Updater. Inherits most + of the functionality from :class:`~.KalmanUpdater`. + + In this case the :meth:`predict_measurement` function uses the + :func:`unscented_transform` function to estimate a (Gaussian) predicted + measurement. This is then updated via the standard Kalman update equations. + + """ + # Can be non-linear and non-differentiable + measurement_model: MeasurementModel = Property( + default=None, + doc="The measurement model to be used. This need not be defined if a " + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") + Nmax: float = Property( + default=10, + doc="maximal number of iterations of SIR") + Nmin: float = Property( + default=5, + doc="minimal number of iterations of stochastic integration rule (SIR)") + Eps: float = Property( + default=5e-3, + doc="allowed threshold for integration error") + SIorder: float = Property( + default=3, + doc="order of SIR (orders 1, 3, 5 are currently supported)") + + @lru_cache() + def predict_measurement(self, predicted_state, measurement_model=None): + """SIF. + + Parameters + ---------- + predicted_state : :class:`~.GaussianStatePrediction` + A predicted state + measurement_model : :class:`~.MeasurementModel`, optional + The measurement model used to generate the measurement prediction. + This should be used in cases where the measurement model is + dependent on the received measurement (the default is `None`, in + which case the updater will use the measurement model specified on + initialisation) + + Returns + ------- + : :class:`~.GaussianMeasurementPrediction` + The measurement prediction + + """ + + measurement_model = self._check_measurement_model(measurement_model) + Sp = np.linalg.cholesky(predicted_state.covar) + nx = len(predicted_state.mean) + nz = measurement_model.ndim; + + epMean = predicted_state.mean; + Iz = np.zeros((nz,1)); + Vz = np.zeros((nz,1)); + IPz = np.zeros((nz)); + VPz = np.zeros((nz)); + IPxz = np.zeros((nx,nz)); + VPxz = np.zeros((nx,nz)); + N = 0 # number of iterations + #- SIR recursion for measurement predictive moments computation + # -- until either required number of iterations is reached or threshold is reached + while N self.Eps)])]): + N = N+1; + # -- cubature points and weights computation (for standard normal PDF) + [SCRSigmaPoints,w] = stochasticCubatureRulePoints(nx,self.SIorder); + + + # -- points transformation for given filtering mean and covariance + # matrix + xpoints = Sp@SCRSigmaPoints + np.matlib.repmat(epMean,1,np.size(SCRSigmaPoints,1)) + # -- points transformation via measurement equation (deterministic part) + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(predicted_state) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + hpoints = StateVectors([ + measurement_model.function(sigma_points_state) for sigma_points_state in sigma_points_states]) + # -- stochastic integration rule for predictive measurement mean and covariance + # matrix and predictive state and measurement covariance matrix + # SumRz = hpoints@w.reshape(np.size(w),1); + SumRz = np.average(hpoints, axis=1, weights=w); + + # --- update mean Iz + Dz = (SumRz-Iz)/N; + Iz = Iz+Dz; + Vz = (N-2)*Vz/N+Dz**2; + + # - measurement predictive moments + zp = Iz; + + + N = 0 + while N self.Eps), (np.linalg.norm(VPxz)> self.Eps)])]): + N = N+1; + # -- cubature points and weights computation (for standard normal PDF) + [SCRSigmaPoints,w] = stochasticCubatureRulePoints(nx,self.SIorder); + + # -- points transformation for given filtering mean and covariance + # matrix + xpoints = Sp@SCRSigmaPoints + np.matlib.repmat(epMean,1,np.size(SCRSigmaPoints,1)) + # -- points transformation via measurement equation (deterministic part) + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(predicted_state) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + hpoints = StateVectors([ + measurement_model.function(sigma_points_state) for sigma_points_state in sigma_points_states]) + # -- stochastic integration rule for predictive measurement mean and covariance + # matrix and predictive state and measurement covariance matrix + + + hpoints_diff = hpoints - zp + SumRPz = hpoints_diff@(np.diag(w))@(hpoints_diff.T); + SumRPxz = ((xpoints-xpoints[:, 0:1]) @ np.diag(w) @ (hpoints_diff).T) + + + # --- update covariance matrix IPz + DPz = (SumRPz-IPz)/N; + IPz = IPz+DPz; + VPz = (N-2)*VPz/N+DPz**2; + # # --- update cross-covariance matrix IPxz + DPxz = (SumRPxz-IPxz)/N; + IPxz = IPxz+DPxz; + VPxz = (N-2)*VPxz/N+DPxz**2; + + Pzp = IPz; + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()); + Pzp = Pzp.astype(np.float64) + + Pxzp = IPxz; + + + cross_covar = Pxzp.view(CovarianceMatrix) + return MeasurementPrediction.from_state( + predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), cross_covar=cross_covar) From 1c4f081cc41436007125dd7f5fd1adb5c96b2177 Mon Sep 17 00:00:00 2001 From: John Hiles Date: Mon, 1 Apr 2024 16:29:32 -0400 Subject: [PATCH 02/51] Brought Stochastic Integration filter in line with Flake8 style guidelines. --- stonesoup/functions/__init__.py | 147 +++++++++++++----------- stonesoup/predictor/kalman.py | 112 +++++++++--------- stonesoup/smoother/kalman.py | 197 ++++++++++++++++---------------- stonesoup/types/state.py | 3 + stonesoup/updater/kalman.py | 125 ++++++++++---------- 5 files changed, 309 insertions(+), 275 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 791f829d7..37526edb7 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -762,7 +762,8 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0): state_x.state_vector = state_x.state_vector + a*delta_t + b@delta_w return state_x.state_vector -def RandOrthMat(n,tol=1e-6): + +def RandOrthMat(n, tol=1e-6): """Random orthogonal real matrix M = RANDORTHMAT(n) @@ -770,11 +771,11 @@ def RandOrthMat(n,tol=1e-6): M = RANDORTHMAT(n,tol) explicitly specifies a thresh value that measures linear dependence of a newly formed column with the existing columns. Defaults to 1e-6. - - In this version the generated matrix distribution *is* uniform over the manifold - O(n) w.r.t. the induced R^(n^2) Lebesgue measure, at a slight computational - overhead (randn + normalization, as opposed to rand ). - + + In this version the generated matrix distribution *is* uniform over the + manifold O(n) w.r.t. the induced R^(n^2) Lebesgue measure,at a slight + computational overhead (randn + normalization, as opposed to rand ). + (c) Ofek Shilon , 2006. Parameters @@ -787,29 +788,28 @@ def RandOrthMat(n,tol=1e-6): numpy.ndarray n x n Random orthogonal real matrix """ - - M = np.zeros((n,n)) # prealloc -# # Gram-schmidt on random column vectors - vi = np.random.rand(n,1) -# # the n-dimensional normal distribution has spherical symmetry, which implies -# # that after normalization the drawn vectors would be uniformly distributed on the -# # n-dimensional unit sphere. - M[:,0] = (vi*1/(np.linalg.norm(vi))).T + M = np.zeros((n, n)) + + # Gram-schmidt on random column vectors + vi = np.random.rand(n, 1) + # the n-dimensional normal distribution has spherical symmetry, which implies + # that after normalization the drawn vectors would be uniformly distributed on the + # n-dimensional unit sphere. + M[:, 0] = (vi * 1 / (np.linalg.norm(vi))).T - for i in range(1,n): + for i in range(1, n): nrm = 0 - while nrm self.Eps), ])]): - N = N+1 + # - SIR recursion for state predictive moments computation + # -- until either max iterations or threshold is reached + while N < self.Nmin or np.all([N < self.Nmax, + np.any([(np.linalg.norm(Vx) > self.Eps), + ])]): + N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints,w] = stochasticCubatureRulePoints(enx,self.SIorder) + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, + self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + npm.repmat(prior.mean,1,np.size(SCRSigmaPoints,1)) + xpoints = Sp@SCRSigmaPoints + \ + npm.repmat(prior.mean, 1, np.size(SCRSigmaPoints, 1)) # -- points transformation via dynamics (deterministic part) # Put these points into s State object list sigma_points_states = [] @@ -591,30 +595,33 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): sigma_points_states.append(state_copy) fpoints = StateVectors([ - transition_and_control_function(sigma_points_state) for sigma_points_state in sigma_points_states]) - # -- stochastic integration rule for predictive state mean and covariance - # matrix - # SumRx = fpoints@w.reshape(np.size(w),1) - SumRx = np.average(fpoints, axis=1, weights=w); - + transition_and_control_function(sigma_points_state) + for sigma_points_state in sigma_points_states]) + # Stochastic integration rule for predictive state mean and + # covariance matrix + SumRx = np.average(fpoints, axis=1, weights=w) + # --- update mean Ix - Dx = (SumRx-Ix)/N - Ix = Ix+Dx - Vx = (N-2)*Vx/N+Dx**2 - + Dx = (SumRx - Ix) / N + Ix = Ix + Dx + Vx = (N - 2) * Vx / N + Dx ** 2 + xp = Ix - N = 0 - # - SIR recursion for state predictive moments computation - # -- until either required number of iterations is reached or threshold is reached - while N self.Eps)])]): - N = N+1 + # - SIR recursion for state predictive moments computation + # -- until max iterations are reached or threshold is reached + while N < self.Nmin or np.all([N < self.Nmax, + np.any([(np.linalg.norm(VPx) > + self.Eps)])]): + N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints,w] = stochasticCubatureRulePoints(enx,self.SIorder) + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, + self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + npm.repmat(prior.mean,1,np.size(SCRSigmaPoints,1)) + xpoints = Sp @ SCRSigmaPoints + \ + npm.repmat(prior.mean, 1, np.size(SCRSigmaPoints, 1)) # -- points transformation via dynamics (deterministic part) # Put these points into s State object list sigma_points_states = [] @@ -624,30 +631,31 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): sigma_points_states.append(state_copy) fpoints = StateVectors([ - transition_and_control_function(sigma_points_state) for sigma_points_state in sigma_points_states]) + transition_and_control_function(sigma_points_state) + for sigma_points_state in sigma_points_states]) # -- stochastic integration rule for predictive state mean and covariance # matrix - + fpoints_diff = fpoints - xp - SumRPx = fpoints_diff@(np.diag(w))@(fpoints_diff.T); + SumRPx = fpoints_diff @ np.diag(w) @ fpoints_diff.T # SumRPx=np.multiply(pom,fpoints)@fpoints.T # --- update covariance matrix IPx - DPx = (SumRPx-IPx)/N - IPx = IPx+DPx - VPx = (N-2)*VPx/N+DPx**2 - - + DPx = (SumRPx - IPx) / N + IPx = IPx + DPx + VPx = (N - 2) * VPx / N + DPx ** 2 + Q = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) - Pp = IPx - Pp = Pp + Q + np.diag(Vx.ravel()); - Pp = (Pp+Pp.T)/2 - + Pp = Pp + Q + np.diag(Vx.ravel()) + Pp = (Pp+Pp.T) / 2 + Pp = Pp.astype(np.float64) - # and return a Gaussian state based on these parameters - return Prediction.from_state(prior, xp.view(StateVector), Pp.view(CovarianceMatrix), timestamp=timestamp, - transition_model=self.transition_model, prior=prior) + return Prediction.from_state(prior, xp.view(StateVector), + Pp.view(CovarianceMatrix), + timestamp=timestamp, + transition_model=self.transition_model, + prior=prior) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index b9a2b386a..10e62661d 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -14,6 +14,7 @@ from ..functions import gauss2sigma, unscented_transform, stochasticCubatureRulePoints from ..types.array import StateVector, StateVectors + class KalmanSmoother(Smoother): r""" The linear-Gaussian or Rauch-Tung-Striebel smoother, colloquially the Kalman smoother [1]_. The @@ -304,102 +305,106 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): sigma_point_states, mean_weights, covar_weights, transition_function) - return cross_covar @ np.linalg.inv(prediction.covar) - + return cross_covar @ np.linalg.inv(prediction.covar) + + class SIFKalmanSmoother(KalmanSmoother): - r"""The stochastic integration version of the Kalman filter. As with the parent version of the Kalman - smoother, the mean and covariance of the prediction are retrieved from the track. The - stochastic integration is used to calculate the smoothing gain. + r""" + The stochastic integration version of the Kalman filter. + As with the parent version of the Kalman smoother, + the mean and covariance of the prediction are retrieved from the track. + The stochastic integration is used to calculate the smoothing gain. + """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") + + Nmax: float = Property( + default=10, + doc="maximal number of iterations of SIR") + Nmin: float = Property( + default=5, + doc="minimal number of iterations of stochastic integration rule (SIR)") + Eps: float = Property( + default=5e-3, + doc="allowed threshold for integration error") + SIorder: float = Property( + default=3, + doc="order of SIR (orders 1, 3, 5 are currently supported)") + + def _smooth_gain(self, state, prediction, time_interval, **kwargs): + """Calculate the smoothing gain + + Parameters + ---------- + state : :class:`~.State` + The input state + prediction : :class:`~.GaussianStatePrediction` + The prediction from the subsequent timestep + + Returns + ------- + : Matrix + The smoothing gain """ - transition_model: TransitionModel = Property(doc="The transition model to be used.") - - Nmax: float = Property( - default=10, - doc="maximal number of iterations of SIR") - Nmin: float = Property( - default=5, - doc="minimal number of iterations of stochastic integration rule (SIR)") - Eps: float = Property( - default=5e-3, - doc="allowed threshold for integration error") - SIorder: float = Property( - default=3, - doc="order of SIR (orders 1, 3, 5 are currently supported)") - - def _smooth_gain(self, state, prediction, time_interval, **kwargs): - """Calculate the smoothing gain - - Parameters - ---------- - state : :class:`~.State` - The input state - prediction : :class:`~.GaussianStatePrediction` - The prediction from the subsequent timestep - - Returns - ------- - : Matrix - The smoothing gain - - """ - - # - initialisation - predMean = prediction.mean - filtMean = state.mean - filtVar = state.covar - - - # This ensures that the time interval is correctly applied. - transition_function = partial( - self._transition_model(state).function, - time_interval=time_interval) - - - nx = predMean.shape[0] - - efMean = filtMean.copy() - efVar = filtVar.copy() - Sf = np.linalg.cholesky(efVar) - IPxx = np.zeros((nx, nx)) - VPxx = np.zeros((nx, nx)) - N = 0 # number of iterations - - # - SIR recursion for measurement predictive moments computation - # -- until either required number of iterations is reached or threshold is reached - while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): - N += 1 - - # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) - - # -- points transformation for given filtering mean and covariance matrix - # xpoints = np.dot(Sf, SCRSigmaPoints) + efMean[:, np.newaxis] - xpoints = Sf@SCRSigmaPoints + np.matlib.repmat(efMean,1,np.size(SCRSigmaPoints,1)) - # -- points transformation via measurement equation (deterministic part) - # fpoints = ffunct(xpoints, wMean, Time) - - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(prediction) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - fpoints = StateVectors([ - transition_function(sigma_points_state) for sigma_points_state in sigma_points_states]) - - # -- stochastic integration rule for predictive measurement mean and covariance matrix - fpoints_diff = fpoints - predMean - xpoints_diff = xpoints - filtMean - SumRPxx = xpoints_diff@(fpoints_diff*np.matlib.repmat(w,nx,1)).T - - - # --- update cross-covariance matrix IPxx - DPxx = (SumRPxx - IPxx) / N - IPxx += DPxx - VPxx = (N - 2) * VPxx / N + DPxx**2 - - # - measurement predictive moments - Pxxps = IPxx - - - return Pxxps @ np.linalg.inv(prediction.covar) + + # - initialisation + predMean = prediction.mean + filtMean = state.mean + filtVar = state.covar + + # This ensures that the time interval is correctly applied. + transition_function = partial( + self._transition_model(state).function, + time_interval=time_interval) + + nx = predMean.shape[0] + + efMean = filtMean.copy() + efVar = filtVar.copy() + Sf = np.linalg.cholesky(efVar) + IPxx = np.zeros((nx, nx)) + VPxx = np.zeros((nx, nx)) + N = 0 # number of iterations + + # - SIR recursion for measurement predictive moments computation + # -- until either required number of iterations is reached or threshold is reached + while N < self.Nmin or all([N < self.Nmax, + (np.linalg.norm(VPxx) > self.Eps)]): + N += 1 + + # -- cubature points and weights computation (for standard normal PDF) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, + self.SIorder) + + # -- points transformation for given filtering mean and covariance matrix + # xpoints = np.dot(Sf, SCRSigmaPoints) + efMean[:, np.newaxis] + xpoints = Sf@SCRSigmaPoints + \ + np.matlib.repmat(efMean, 1, np.size(SCRSigmaPoints, 1)) + # -- points transformation via measurement equation (deterministic part) + # fpoints = ffunct(xpoints, wMean, Time) + + sigma_points_states = [] + for xpoint in xpoints.T: + state_copy = copy.copy(prediction) + state_copy.state_vector = StateVector(xpoint) + sigma_points_states.append(state_copy) + fpoints = StateVectors([ + transition_function(sigma_points_state) + for sigma_points_state in sigma_points_states]) + + # Stochastic integration rule for predictive measurement + # mean and covariance matrix + fpoints_diff = fpoints - predMean + xpoints_diff = xpoints - filtMean + SumRPxx = xpoints_diff @ (fpoints_diff * + np.matlib.repmat(w, nx, 1)).T + + # --- update cross-covariance matrix IPxx + DPxx = (SumRPxx - IPxx) / N + IPxx += DPxx + VPxx = (N - 2) * VPxx / N + DPxx ** 2 + + # - measurement predictive moments + Pxxps = IPxx + + return Pxxps @ np.linalg.inv(prediction.covar) diff --git a/stonesoup/types/state.py b/stonesoup/types/state.py index 9374d1ddb..51af23d5c 100644 --- a/stonesoup/types/state.py +++ b/stonesoup/types/state.py @@ -1008,6 +1008,9 @@ def mean(self): @clearable_cached_property('state_vector') def covar(self): """Sample covariance matrix for ensemble""" + print(type(self.state_vector)) + print(self.ndim) + print(self.num_vectors) return np.cov(self.state_vector) @clearable_cached_property('state_vector') diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index d61c4c7a4..731a4c88a 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -16,6 +16,7 @@ from ..functions import gauss2sigma, unscented_transform, stochasticCubatureRulePoints from ..measures import Measure, Euclidean + class KalmanUpdater(Updater): r"""A class which embodies Kalman-type updaters; also a class which performs measurement update step as in the standard Kalman filter. @@ -797,6 +798,7 @@ def _posterior_covariance(self, hypothesis): return post_cov.view(CovarianceMatrix), kalman_gain + class SIFKalmanUpdater(KalmanUpdater): """The SIF version of the Kalman Updater. Inherits most of the functionality from :class:`~.KalmanUpdater`. @@ -847,95 +849,102 @@ def predict_measurement(self, predicted_state, measurement_model=None): The measurement prediction """ - + measurement_model = self._check_measurement_model(measurement_model) Sp = np.linalg.cholesky(predicted_state.covar) nx = len(predicted_state.mean) - nz = measurement_model.ndim; - - epMean = predicted_state.mean; - Iz = np.zeros((nz,1)); - Vz = np.zeros((nz,1)); - IPz = np.zeros((nz)); - VPz = np.zeros((nz)); - IPxz = np.zeros((nx,nz)); - VPxz = np.zeros((nx,nz)); - N = 0 # number of iterations - #- SIR recursion for measurement predictive moments computation - # -- until either required number of iterations is reached or threshold is reached - while N self.Eps)])]): - N = N+1; + nz = measurement_model.ndim + + epMean = predicted_state.mean + Iz = np.zeros((nz, 1)) + Vz = np.zeros((nz, 1)) + IPz = np.zeros((nz)) + VPz = np.zeros((nz)) + IPxz = np.zeros((nx, nz)) + VPxz = np.zeros((nx, nz)) + N = 0 + # SIR recursion for measurement predictive moments computation + # until either number of iterations is reached or threshold is reached + while N < self.Nmin or np.all([N < self.Nmax, + np.any([(np.linalg.norm(Vz) > + self.Eps)])]): + N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints,w] = stochasticCubatureRulePoints(nx,self.SIorder); - - + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, + self.SIorder) + # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + np.matlib.repmat(epMean,1,np.size(SCRSigmaPoints,1)) + xpoints = Sp@SCRSigmaPoints + \ + np.matlib.repmat(epMean, 1, np.size(SCRSigmaPoints, 1)) # -- points transformation via measurement equation (deterministic part) sigma_points_states = [] for xpoint in xpoints.T: state_copy = copy.copy(predicted_state) state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) + hpoints = StateVectors([ - measurement_model.function(sigma_points_state) for sigma_points_state in sigma_points_states]) + measurement_model.function(sigma_points_state) + for sigma_points_state in sigma_points_states]) # -- stochastic integration rule for predictive measurement mean and covariance # matrix and predictive state and measurement covariance matrix # SumRz = hpoints@w.reshape(np.size(w),1); - SumRz = np.average(hpoints, axis=1, weights=w); - + + SumRz = np.average(hpoints, axis=1, weights=w) + # --- update mean Iz - Dz = (SumRz-Iz)/N; - Iz = Iz+Dz; - Vz = (N-2)*Vz/N+Dz**2; - + Dz = (SumRz - Iz) / N + Iz = Iz + Dz + Vz = (N - 2) * Vz / N + Dz ** 2 + # - measurement predictive moments - zp = Iz; - - - N = 0 - while N self.Eps), (np.linalg.norm(VPxz)> self.Eps)])]): - N = N+1; + zp = Iz + + N = 0 + while N < self.Nmin or np.all([N < self.Nmax, + np.any([(np.linalg.norm(VPz) > self.Eps), + (np.linalg.norm(VPxz) > self.Eps)])]): + N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints,w] = stochasticCubatureRulePoints(nx,self.SIorder); - + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, + self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + np.matlib.repmat(epMean,1,np.size(SCRSigmaPoints,1)) - # -- points transformation via measurement equation (deterministic part) + xpoints = Sp@SCRSigmaPoints + \ + np.matlib.repmat(epMean, 1, np.size(SCRSigmaPoints, 1)) + # Points transformation via measurement equation (deterministic part) sigma_points_states = [] for xpoint in xpoints.T: state_copy = copy.copy(predicted_state) state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) hpoints = StateVectors([ - measurement_model.function(sigma_points_state) for sigma_points_state in sigma_points_states]) - # -- stochastic integration rule for predictive measurement mean and covariance - # matrix and predictive state and measurement covariance matrix - - + measurement_model.function(sigma_points_state) + for sigma_points_state in sigma_points_states]) + # Stochastic integration rule for predictive measurement mean and covariance + # Matrix and predictive state and measurement covariance matrix + hpoints_diff = hpoints - zp - SumRPz = hpoints_diff@(np.diag(w))@(hpoints_diff.T); + SumRPz = hpoints_diff@(np.diag(w))@(hpoints_diff.T) SumRPxz = ((xpoints-xpoints[:, 0:1]) @ np.diag(w) @ (hpoints_diff).T) - - - # --- update covariance matrix IPz - DPz = (SumRPz-IPz)/N; - IPz = IPz+DPz; - VPz = (N-2)*VPz/N+DPz**2; - # # --- update cross-covariance matrix IPxz - DPxz = (SumRPxz-IPxz)/N; - IPxz = IPxz+DPxz; - VPxz = (N-2)*VPxz/N+DPxz**2; - - Pzp = IPz; - Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()); + + # Update covariance matrix IPz + DPz = (SumRPz-IPz)/N + IPz = IPz+DPz + VPz = (N-2)*VPz/N+DPz**2 + # Update cross-covariance matrix IPxz + DPxz = (SumRPxz-IPxz)/N + IPxz = IPxz+DPxz + VPxz = (N-2)*VPxz/N+DPxz**2 + + Pzp = IPz + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) Pzp = Pzp.astype(np.float64) - Pxzp = IPxz; + Pxzp = IPxz - cross_covar = Pxzp.view(CovarianceMatrix) return MeasurementPrediction.from_state( - predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), cross_covar=cross_covar) + predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), + cross_covar=cross_covar) From c7429441364d744c9b8c448427e8ab50dc7f7bc3 Mon Sep 17 00:00:00 2001 From: John Hiles Date: Sun, 28 Apr 2024 22:57:38 -0400 Subject: [PATCH 03/51] Added unit tests, changed names for stochastic integration predictor and updaters. --- stonesoup/functions/tests/test_functions.py | 5 +++++ stonesoup/predictor/kalman.py | 4 ++-- stonesoup/predictor/tests/test_kalman.py | 13 ++++++++++--- stonesoup/updater/kalman.py | 21 +++++++++++---------- stonesoup/updater/tests/test_kalman.py | 16 ++++++++++++++-- 5 files changed, 42 insertions(+), 17 deletions(-) diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index 2b846ec4d..3de20b53a 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -332,3 +332,8 @@ def test_gm_sample(means, covars, weights, size): assert samples.shape[0] == means[0].shape[0] else: assert samples.shape[0] == means.shape[0] + +#def test_random_orthogonal_matrix() + +#def test_stochasticCubatureRulePoints(): + diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 4505753ef..853887987 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -481,8 +481,8 @@ def _predicted_covariance(self, prior, predict_over_interval, control_input=None ctrl_mat@sqrt_ctrl_noi@sqrt_ctrl_noi.T@ctrl_mat.T) -class SIFKalmanPredictor(KalmanPredictor): - """StochasticIntegrationKalmanFilter class +class StochasticIntegrationPredictor(KalmanPredictor): + """Stochastic Integration Kalman Filter class The state prediction is accomplished by stochastic integration """ diff --git a/stonesoup/predictor/tests/test_kalman.py b/stonesoup/predictor/tests/test_kalman.py index a6eab49f7..1e3007f57 100644 --- a/stonesoup/predictor/tests/test_kalman.py +++ b/stonesoup/predictor/tests/test_kalman.py @@ -5,7 +5,7 @@ from ...models.transition.linear import ConstantVelocity from ...predictor.kalman import ( KalmanPredictor, ExtendedKalmanPredictor, UnscentedKalmanPredictor, - SqrtKalmanPredictor) + SqrtKalmanPredictor, StochasticIntegrationPredictor) from ...types.prediction import GaussianStatePrediction from ...types.state import GaussianState, SqrtGaussianState from ...types.track import Track @@ -34,9 +34,16 @@ np.array([[-6.45], [0.7]]), np.array([[4.1123, 0.0013], [0.0013, 0.0365]]) - ) + ), + ( # Stochastic Integration Filter + StochasticIntegrationPredictor, + ConstantVelocity(noise_diff_coeff=0.1), + np.array([[-6.45], [0.7]]), + np.array([[4.1123, 0.0013], + [0.0013, 0.0365]]) + ), ], - ids=["standard", "extended", "unscented"] + ids=["standard", "extended", "unscented", "stochasticIntegration"] ) def test_kalman(PredictorClass, transition_model, prior_mean, prior_covar): diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 731a4c88a..09d0df53e 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -799,8 +799,8 @@ def _posterior_covariance(self, hypothesis): return post_cov.view(CovarianceMatrix), kalman_gain -class SIFKalmanUpdater(KalmanUpdater): - """The SIF version of the Kalman Updater. Inherits most +class StochasticIntegrationUpdater(KalmanUpdater): + """Stochastic Integration Kalman Filter class. Inherits most of the functionality from :class:`~.KalmanUpdater`. In this case the :meth:`predict_measurement` function uses the @@ -926,17 +926,18 @@ def predict_measurement(self, predicted_state, measurement_model=None): # Matrix and predictive state and measurement covariance matrix hpoints_diff = hpoints - zp - SumRPz = hpoints_diff@(np.diag(w))@(hpoints_diff.T) - SumRPxz = ((xpoints-xpoints[:, 0:1]) @ np.diag(w) @ (hpoints_diff).T) + SumRPz = hpoints_diff @ np.diag(w) @ (hpoints_diff.T) + SumRPxz = ((xpoints - xpoints[:, 0:1]) @ np.diag(w) @ + (hpoints_diff).T) # Update covariance matrix IPz - DPz = (SumRPz-IPz)/N - IPz = IPz+DPz - VPz = (N-2)*VPz/N+DPz**2 + DPz = (SumRPz - IPz) / N + IPz = IPz + DPz + VPz = (N - 2) * VPz / N + DPz ** 2 # Update cross-covariance matrix IPxz - DPxz = (SumRPxz-IPxz)/N - IPxz = IPxz+DPxz - VPxz = (N-2)*VPxz/N+DPxz**2 + DPxz = (SumRPxz - IPxz) / N + IPxz = IPxz + DPxz + VPxz = (N - 2) * VPxz / N + DPxz ** 2 Pzp = IPz Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index 40f62248c..3ff8acd36 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -14,7 +14,8 @@ UnscentedKalmanUpdater, SqrtKalmanUpdater, IteratedKalmanUpdater, - SchmidtKalmanUpdater) + SchmidtKalmanUpdater, + StochasticIntegrationUpdater) @pytest.mark.parametrize( @@ -65,8 +66,18 @@ [0.0013, 0.0365]])), Detection(np.array([[-6.23]])) ), + ( # Stochastic Integration + StochasticIntegrationUpdater, + LinearGaussian(ndim_state=2, mapping=[0], + noise_covar=np.array([[0.04]])), + GaussianStatePrediction(np.array([[-6.45], [0.7]]), + np.array([[4.1123, 0.0013], + [0.0013, 0.0365]])), + Detection(np.array([[-6.23]])) + ) ], - ids=["standard", "extended", "unscented", "iterated", "schmidtkalman"] + ids=["standard", "extended", "unscented", "iterated", "schmidtkalman", + "stochasticIntegration"] ) def test_kalman(UpdaterClass, measurement_model, prediction, measurement): @@ -250,3 +261,4 @@ def test_schmidtkalman(): assert np.allclose(update.mean, sk_update.mean) assert np.allclose(update.covar, sk_update.covar) + From 0376ce08452cf940b28b875c3ca2c5fd2207c481 Mon Sep 17 00:00:00 2001 From: John Hiles Date: Mon, 29 Apr 2024 16:50:59 -0400 Subject: [PATCH 04/51] Revised Stochastic Integration predictor and updater docstrings. --- stonesoup/predictor/kalman.py | 3 ++- stonesoup/updater/kalman.py | 5 ++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 853887987..c213a75f6 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -484,7 +484,8 @@ def _predicted_covariance(self, prior, predict_over_interval, control_input=None class StochasticIntegrationPredictor(KalmanPredictor): """Stochastic Integration Kalman Filter class - The state prediction is accomplished by stochastic integration + The state prediction of nonlinear models is accomplished by the stochastic + integration approximation. """ transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 09d0df53e..05fb78a6a 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -803,9 +803,8 @@ class StochasticIntegrationUpdater(KalmanUpdater): """Stochastic Integration Kalman Filter class. Inherits most of the functionality from :class:`~.KalmanUpdater`. - In this case the :meth:`predict_measurement` function uses the - :func:`unscented_transform` function to estimate a (Gaussian) predicted - measurement. This is then updated via the standard Kalman update equations. + The measurement update of nonlinear measurement models is accomplished by + the stochastic integration approximation. """ # Can be non-linear and non-differentiable From 83204f8fa8ec1226b73a62fe0c6dd2a777fc833b Mon Sep 17 00:00:00 2001 From: Osmium Date: Tue, 11 Jun 2024 18:35:25 -0400 Subject: [PATCH 05/51] Fixed flake8 violations introduced in merg --- stonesoup/functions/__init__.py | 1 - stonesoup/functions/tests/test_functions.py | 4 +- stonesoup/predictor/kalman.py | 83 +++++++++++++++------ stonesoup/predictor/tests/test_kalman.py | 12 ++- stonesoup/updater/kalman.py | 56 ++++++++++---- stonesoup/updater/tests/test_kalman.py | 70 +---------------- 6 files changed, 113 insertions(+), 113 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 7e0417191..38617be4d 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1083,4 +1083,3 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. cross_covar = cross_covar.view(CovarianceMatrix) return mean, covar, cross_covar, cubature_points_t - diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index fff94fd2e..057bb7bde 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -346,6 +346,7 @@ def test_gm_sample(means, covars, weights, size): else: assert samples.shape[0] == means.shape[0] + @pytest.mark.parametrize( "mean, covar, alp", [ @@ -357,8 +358,6 @@ def test_gm_sample(means, covars, weights, size): [0, 0.06, -0.01, 1.1]]), 0.7) ] ) - - def test_cubature_transform(mean, covar, alp): instate = GaussianState(mean, covar) @@ -381,4 +380,3 @@ def identity_function(inpu): assert np.allclose(outcovar, instate.covar) assert np.allclose(mean, instate.state_vector) assert np.allclose(covar, instate.covar) - diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index fd3fcaffc..088ea018e 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -15,8 +15,8 @@ from ..models.transition.linear import LinearGaussianTransitionModel from ..models.control import ControlModel from ..models.control.linear import LinearControlModel -from ..functions import gauss2sigma, unscented_transform, cubature_transform, stochasticCubatureRulePoints - +from ..functions import gauss2sigma, unscented_transform, cubature_transform, \ + stochasticCubatureRulePoints class KalmanPredictor(Predictor): @@ -496,11 +496,10 @@ class CubatureKalmanPredictor(KalmanPredictor): default=None, doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.") - alpha: float = Property( - default=1.0, - doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + default=1.0, + doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " + "vice versa.") def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions @@ -544,12 +543,26 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted state covariance :math:`P_{k|k-1}` """ + + # Get the prediction interval + predict_over_interval = self._predict_over_interval(prior, timestamp) + + # The covariance on the transition model + the control model + # TODO: See equivalent note to unscented transform. + ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) + ctrl_noi = self.control_model.covar(**kwargs) + total_noise_covar = \ + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ + + ctrl_mat@ctrl_noi@ctrl_mat.T + + # This ensures that function passed to transform has the correct time interval and control + # input transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, time_interval=predict_over_interval) -x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, + x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, covar_noise=total_noise_covar, alpha=self.alpha) return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, @@ -561,7 +574,12 @@ class StochasticIntegrationPredictor(KalmanPredictor): The state prediction of nonlinear models is accomplished by the stochastic integration approximation. - + """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") + control_model: ControlModel = Property( + default=None, + doc="The control model to be used. Default `None` where the predictor " + "will create a zero-effect linear :class:`~.ControlModel`.") Nmax: float = Property( default=10, doc="maximal number of iterations of SIR") @@ -584,14 +602,45 @@ def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions for the unscented transform + Parameters + ---------- + prior_state_vector : :class:`~.State` + Prior state vector + **kwargs : various, optional + These are passed to :class:`~.TransitionModel.function` + + Returns + ------- + : :class:`numpy.ndarray` + The combined, noiseless, effect of applying the transition and + control + """ + return self.transition_model.function(prior_state, **kwargs) + \ self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): r"""The stochastic integration version of the predict step + + Parameters + ---------- + prior : :class:`~.State` + Prior state, :math:`\mathbf{x}_{k-1}` + timestamp : :class:`datetime.datetime` + Time to transit to (:math:`k`) + control_input: :class:`~.State` + Control input vector, :math:`\mathbf{u}_k` + **kwargs : various, optional These are passed to :meth:`~.TransitionModel.covar` + + Returns + ------- + : :class:`~.GaussianStatePrediction` + The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted + state covariance :math:`P_{k|k-1}` """ + nx = np.size(prior.mean, 0) enx = nx @@ -608,20 +657,11 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # This ensures that function passed to unscented transform has the # correct time interval - - # Get the prediction interval - predict_over_interval = self._predict_over_interval(prior, timestamp) - - # The covariance on the transition model + the control model - # TODO: See equivalent note to unscented transform. - ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) - ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = \ - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ - + ctrl_mat@ctrl_noi@ctrl_mat.T + transition_and_control_function = partial( + self._transition_and_control_function, + control_input=control_input, + time_interval=predict_over_interval) - # This ensures that function passed to transform has the correct time interval and control - # input # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached while N < self.Nmin or np.all([N < self.Nmax, @@ -708,4 +748,3 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): timestamp=timestamp, transition_model=self.transition_model, prior=prior) - diff --git a/stonesoup/predictor/tests/test_kalman.py b/stonesoup/predictor/tests/test_kalman.py index ef8879f23..24bbf3ef4 100644 --- a/stonesoup/predictor/tests/test_kalman.py +++ b/stonesoup/predictor/tests/test_kalman.py @@ -5,7 +5,7 @@ from ...models.transition.linear import ConstantVelocity from ...predictor.kalman import ( KalmanPredictor, ExtendedKalmanPredictor, UnscentedKalmanPredictor, - SqrtKalmanPredictor, CubatureKalmanPredictor,StochasticIntegrationPredictor) + SqrtKalmanPredictor, CubatureKalmanPredictor, StochasticIntegrationPredictor) from ...types.prediction import GaussianStatePrediction from ...types.state import GaussianState, SqrtGaussianState from ...types.track import Track @@ -35,8 +35,6 @@ np.array([[4.1123, 0.0013], [0.0013, 0.0365]]) ), - ( # Stochastic Integration Filter - StochasticIntegrationPredictor, ( # cubature Kalman CubatureKalmanPredictor, ConstantVelocity(noise_diff_coeff=0.1), @@ -44,9 +42,15 @@ np.array([[4.1123, 0.0013], [0.0013, 0.0365]]) ), + ( # Stochastic Integration + StochasticIntegrationPredictor, + ConstantVelocity(noise_diff_coeff=0.1), + np.array([[-6.45], [0.7]]), + np.array([[4.1123, 0.0013], + [0.0013, 0.0365]]) + ) ], ids=["standard", "extended", "unscented", "cubature", "stochasticIntegration"] - ) ) def test_kalman(PredictorClass, transition_model, prior_mean, prior_covar): diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index c9d7bb76b..cd0e8c251 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -13,7 +13,8 @@ from ..models.base import LinearModel from ..models.measurement.linear import LinearGaussian from ..models.measurement import MeasurementModel -from ..functions import gauss2sigma, unscented_transform, cubature_transform, stochasticCubatureRulePoints +from ..functions import gauss2sigma, unscented_transform, cubature_transform, \ + stochasticCubatureRulePoints from ..measures import Measure, Euclidean @@ -861,6 +862,16 @@ class CubatureKalmanUpdater(KalmanUpdater): "measurement model is provided in the measurement. If no model " "specified on construction, or in the measurement, then error " "will be thrown.") + alpha: float = Property( + default=1.0, + doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " + "vice versa.") + + @lru_cache() + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, + **kwargs): + """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to + estimate a Gauss-distributed predicted measurement. Parameters ---------- @@ -872,12 +883,16 @@ class CubatureKalmanUpdater(KalmanUpdater): dependent on the received measurement (the default is `None`, in which case the updater will use the measurement model specified on initialisation) + measurement_noise : bool + Whether to include measurement noise :math:`R` with innovation covariance. + Default `True` Returns ------- : :class:`~.GaussianMeasurementPrediction` The measurement prediction + """ measurement_model = self._check_measurement_model(measurement_model) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None @@ -899,6 +914,12 @@ class StochasticIntegrationUpdater(KalmanUpdater): """ # Can be non-linear and non-differentiable + measurement_model: MeasurementModel = Property( + default=None, + doc="The measurement model to be used. This need not be defined if a " + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") Nmax: float = Property( default=10, doc="maximal number of iterations of SIR") @@ -906,29 +927,34 @@ class StochasticIntegrationUpdater(KalmanUpdater): default=5, doc="minimal number of iterations of stochastic integration rule (SIR)") Eps: float = Property( - default=5e-3, + default=5e-4, doc="allowed threshold for integration error") SIorder: float = Property( default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)") @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None): + def predict_measurement(self, predicted_state, measurement_model=None, + **kwargs): """SIF. - alpha: float = Property( - default=1.0, - doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + Parameters + ---------- + predicted_state : :class:`~.GaussianStatePrediction` + A predicted state + measurement_model : :class:`~.MeasurementModel`, optional + The measurement model used to generate the measurement prediction. + This should be used in cases where the measurement model is + dependent on the received measurement (the default is `None`, in + which case the updater will use the measurement model specified on + initialisation) - @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): - """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to - estimate a Gauss-distributed predicted measurement. - measurement_noise : bool - Whether to include measurement noise :math:`R` with innovation covariance. - Default `True` + Returns + ------- + : :class:`~.GaussianMeasurementPrediction` + The measurement prediction + + """ measurement_model = self._check_measurement_model(measurement_model) Sp = np.linalg.cholesky(predicted_state.covar) diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index eb688ddf6..a5b0ea6cf 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -1,5 +1,3 @@ -"""Test for updater.kalman module""" - import pytest import numpy as np @@ -15,71 +13,8 @@ SqrtKalmanUpdater, IteratedKalmanUpdater, SchmidtKalmanUpdater, - CubatureKalmanUpdater, - StochasticIntegrationUpdater) - -@pytest.mark.parametrize( - "UpdaterClass, measurement_model, prediction, measurement", - [ - ( # Standard Kalman - KalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Extended Kalman - ExtendedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Unscented Kalman - UnscentedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Iterated Kalman - IteratedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Schmidt Kalman - SchmidtKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Stochastic Integration - StochasticIntegrationUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ) - ], - ids=["standard", "extended", "unscented", "iterated", "schmidtkalman", - "stochasticIntegration"] -) -def test_kalman(UpdaterClass, measurement_model, prediction, measurement): + CubatureKalmanUpdater) + @pytest.fixture(params=[KalmanUpdater, ExtendedKalmanUpdater, UnscentedKalmanUpdater, IteratedKalmanUpdater, SchmidtKalmanUpdater, CubatureKalmanUpdater]) @@ -292,4 +227,3 @@ def test_schmidtkalman(): assert np.allclose(update.mean, sk_update.mean) assert np.allclose(update.covar, sk_update.covar) - From 9e66dbd3c9f150780c49cd83ff5d4a0b1dd93cc8 Mon Sep 17 00:00:00 2001 From: Osmium Date: Tue, 25 Jun 2024 11:26:55 -0400 Subject: [PATCH 06/51] Adding unit tests for updater. --- stonesoup/predictor/kalman.py | 85 +++++++++++++++++------- stonesoup/predictor/tests/test_kalman.py | 14 ++-- stonesoup/updater/kalman.py | 55 ++++++++++----- stonesoup/updater/tests/test_kalman.py | 66 +----------------- 4 files changed, 113 insertions(+), 107 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index fd3fcaffc..78abf1f97 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -15,8 +15,8 @@ from ..models.transition.linear import LinearGaussianTransitionModel from ..models.control import ControlModel from ..models.control.linear import LinearControlModel -from ..functions import gauss2sigma, unscented_transform, cubature_transform, stochasticCubatureRulePoints - +from ..functions import gauss2sigma, unscented_transform, cubature_transform, \ + stochasticCubatureRulePoints class KalmanPredictor(Predictor): @@ -496,11 +496,10 @@ class CubatureKalmanPredictor(KalmanPredictor): default=None, doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.") - alpha: float = Property( - default=1.0, - doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + default=1.0, + doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " + "vice versa.") def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions @@ -544,12 +543,26 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted state covariance :math:`P_{k|k-1}` """ + + # Get the prediction interval + predict_over_interval = self._predict_over_interval(prior, timestamp) + + # The covariance on the transition model + the control model + # TODO: See equivalent note to unscented transform. + ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) + ctrl_noi = self.control_model.covar(**kwargs) + total_noise_covar = \ + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ + + ctrl_mat@ctrl_noi@ctrl_mat.T + + # This ensures that function passed to transform has the correct time interval and control + # input transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, time_interval=predict_over_interval) -x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, + x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, covar_noise=total_noise_covar, alpha=self.alpha) return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, @@ -561,7 +574,12 @@ class StochasticIntegrationPredictor(KalmanPredictor): The state prediction of nonlinear models is accomplished by the stochastic integration approximation. - + """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") + control_model: ControlModel = Property( + default=None, + doc="The control model to be used. Default `None` where the predictor " + "will create a zero-effect linear :class:`~.ControlModel`.") Nmax: float = Property( default=10, doc="maximal number of iterations of SIR") @@ -584,14 +602,45 @@ def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions for the unscented transform + Parameters + ---------- + prior_state_vector : :class:`~.State` + Prior state vector + **kwargs : various, optional + These are passed to :class:`~.TransitionModel.function` + + Returns + ------- + : :class:`numpy.ndarray` + The combined, noiseless, effect of applying the transition and + control + """ + return self.transition_model.function(prior_state, **kwargs) + \ self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): r"""The stochastic integration version of the predict step + + Parameters + ---------- + prior : :class:`~.State` + Prior state, :math:`\mathbf{x}_{k-1}` + timestamp : :class:`datetime.datetime` + Time to transit to (:math:`k`) + control_input: :class:`~.State` + Control input vector, :math:`\mathbf{u}_k` + **kwargs : various, optional These are passed to :meth:`~.TransitionModel.covar` + + Returns + ------- + : :class:`~.GaussianStatePrediction` + The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted + state covariance :math:`P_{k|k-1}` """ + nx = np.size(prior.mean, 0) enx = nx @@ -608,20 +657,11 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # This ensures that function passed to unscented transform has the # correct time interval - - # Get the prediction interval - predict_over_interval = self._predict_over_interval(prior, timestamp) - - # The covariance on the transition model + the control model - # TODO: See equivalent note to unscented transform. - ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) - ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = \ - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ - + ctrl_mat@ctrl_noi@ctrl_mat.T + transition_and_control_function = partial( + self._transition_and_control_function, + control_input=control_input, + time_interval=predict_over_interval) - # This ensures that function passed to transform has the correct time interval and control - # input # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached while N < self.Nmin or np.all([N < self.Nmax, @@ -707,5 +747,4 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): Pp.view(CovarianceMatrix), timestamp=timestamp, transition_model=self.transition_model, - prior=prior) - + prior=prior) \ No newline at end of file diff --git a/stonesoup/predictor/tests/test_kalman.py b/stonesoup/predictor/tests/test_kalman.py index ef8879f23..c7aab5a80 100644 --- a/stonesoup/predictor/tests/test_kalman.py +++ b/stonesoup/predictor/tests/test_kalman.py @@ -5,7 +5,7 @@ from ...models.transition.linear import ConstantVelocity from ...predictor.kalman import ( KalmanPredictor, ExtendedKalmanPredictor, UnscentedKalmanPredictor, - SqrtKalmanPredictor, CubatureKalmanPredictor,StochasticIntegrationPredictor) + SqrtKalmanPredictor, CubatureKalmanPredictor, StochasticIntegrationPredictor) from ...types.prediction import GaussianStatePrediction from ...types.state import GaussianState, SqrtGaussianState from ...types.track import Track @@ -35,8 +35,6 @@ np.array([[4.1123, 0.0013], [0.0013, 0.0365]]) ), - ( # Stochastic Integration Filter - StochasticIntegrationPredictor, ( # cubature Kalman CubatureKalmanPredictor, ConstantVelocity(noise_diff_coeff=0.1), @@ -44,9 +42,15 @@ np.array([[4.1123, 0.0013], [0.0013, 0.0365]]) ), + ( # Stochastic Integration + StochasticIntegrationPredictor, + ConstantVelocity(noise_diff_coeff=0.1), + np.array([[-6.45], [0.7]]), + np.array([[4.1123, 0.0013], + [0.0013, 0.0365]]) + ) ], ids=["standard", "extended", "unscented", "cubature", "stochasticIntegration"] - ) ) def test_kalman(PredictorClass, transition_model, prior_mean, prior_covar): @@ -145,4 +149,4 @@ def test_sqrt_kalman(): sqrt_prediction.sqrt_covar@sqrt_prediction.sqrt_covar.T, 0, atol=1.e-14) assert np.allclose(prediction.covar, sqrt_prediction.covar, 0, atol=1.e-14) - assert prediction.timestamp == sqrt_prediction.timestamp + assert prediction.timestamp == sqrt_prediction.timestamp \ No newline at end of file diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index c9d7bb76b..7a261354f 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -861,6 +861,16 @@ class CubatureKalmanUpdater(KalmanUpdater): "measurement model is provided in the measurement. If no model " "specified on construction, or in the measurement, then error " "will be thrown.") + alpha: float = Property( + default=1.0, + doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " + "vice versa.") + + @lru_cache() + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, + **kwargs): + """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to + estimate a Gauss-distributed predicted measurement. Parameters ---------- @@ -872,12 +882,16 @@ class CubatureKalmanUpdater(KalmanUpdater): dependent on the received measurement (the default is `None`, in which case the updater will use the measurement model specified on initialisation) + measurement_noise : bool + Whether to include measurement noise :math:`R` with innovation covariance. + Default `True` Returns ------- : :class:`~.GaussianMeasurementPrediction` The measurement prediction + """ measurement_model = self._check_measurement_model(measurement_model) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None @@ -899,6 +913,12 @@ class StochasticIntegrationUpdater(KalmanUpdater): """ # Can be non-linear and non-differentiable + measurement_model: MeasurementModel = Property( + default=None, + doc="The measurement model to be used. This need not be defined if a " + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") Nmax: float = Property( default=10, doc="maximal number of iterations of SIR") @@ -906,29 +926,34 @@ class StochasticIntegrationUpdater(KalmanUpdater): default=5, doc="minimal number of iterations of stochastic integration rule (SIR)") Eps: float = Property( - default=5e-3, + default=5e-4, doc="allowed threshold for integration error") SIorder: float = Property( default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)") @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None): + def predict_measurement(self, predicted_state, measurement_model=None, + **kwargs): """SIF. - alpha: float = Property( - default=1.0, - doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + Parameters + ---------- + predicted_state : :class:`~.GaussianStatePrediction` + A predicted state + measurement_model : :class:`~.MeasurementModel`, optional + The measurement model used to generate the measurement prediction. + This should be used in cases where the measurement model is + dependent on the received measurement (the default is `None`, in + which case the updater will use the measurement model specified on + initialisation) - @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): - """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to - estimate a Gauss-distributed predicted measurement. - measurement_noise : bool - Whether to include measurement noise :math:`R` with innovation covariance. - Default `True` + Returns + ------- + : :class:`~.GaussianMeasurementPrediction` + The measurement prediction + + """ measurement_model = self._check_measurement_model(measurement_model) Sp = np.linalg.cholesky(predicted_state.covar) @@ -1028,4 +1053,4 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme cross_covar = Pxzp.view(CovarianceMatrix) return MeasurementPrediction.from_state( predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), - cross_covar=cross_covar) + cross_covar=cross_covar) \ No newline at end of file diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index eb688ddf6..836d8da5e 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -18,71 +18,10 @@ CubatureKalmanUpdater, StochasticIntegrationUpdater) -@pytest.mark.parametrize( - "UpdaterClass, measurement_model, prediction, measurement", - [ - ( # Standard Kalman - KalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Extended Kalman - ExtendedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Unscented Kalman - UnscentedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Iterated Kalman - IteratedKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Schmidt Kalman - SchmidtKalmanUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ), - ( # Stochastic Integration - StochasticIntegrationUpdater, - LinearGaussian(ndim_state=2, mapping=[0], - noise_covar=np.array([[0.04]])), - GaussianStatePrediction(np.array([[-6.45], [0.7]]), - np.array([[4.1123, 0.0013], - [0.0013, 0.0365]])), - Detection(np.array([[-6.23]])) - ) - ], - ids=["standard", "extended", "unscented", "iterated", "schmidtkalman", - "stochasticIntegration"] -) -def test_kalman(UpdaterClass, measurement_model, prediction, measurement): @pytest.fixture(params=[KalmanUpdater, ExtendedKalmanUpdater, UnscentedKalmanUpdater, - IteratedKalmanUpdater, SchmidtKalmanUpdater, CubatureKalmanUpdater]) + IteratedKalmanUpdater, SchmidtKalmanUpdater, CubatureKalmanUpdater, + StochasticIntegrationUpdater]) def updater_class(request): return request.param @@ -174,7 +113,6 @@ def test_kalman(updater_class, use_joseph_cov): assert np.array_equal(posterior.hypothesis.measurement, measurement) assert posterior.timestamp == prediction.timestamp - def test_sqrt_kalman(): measurement_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])) From ec1985cd5158023eaf206ba841b1c656a420629e Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 1 Jul 2024 09:56:05 -0400 Subject: [PATCH 07/51] Fixed Stochastic Integration updater not passing unit test --- stonesoup/updater/kalman.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 7a261354f..e03144f6e 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -933,7 +933,7 @@ class StochasticIntegrationUpdater(KalmanUpdater): doc="order of SIR (orders 1, 3, 5 are currently supported)") @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs): """SIF. @@ -1045,7 +1045,10 @@ def predict_measurement(self, predicted_state, measurement_model=None, VPxz = (N - 2) * VPxz / N + DPxz ** 2 Pzp = IPz - Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) + if measurement_noise: + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) + else: + Pzp = Pzp + np.diag(Vz.ravel()) Pzp = Pzp.astype(np.float64) Pxzp = IPxz From 41a1d601e661075dca615038027657a46ac2be43 Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 22 Jul 2024 17:15:42 -0400 Subject: [PATCH 08/51] Added basic unit test for stochastic integration rule. --- stonesoup/functions/tests/test_functions.py | 33 ++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index fff94fd2e..88f279083 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -7,7 +7,7 @@ from .. import ( cholesky_eps, jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma, rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart, dotproduct, gm_sample, - gauss2cubature, cubature2gauss, cubature_transform) + gauss2cubature, cubature2gauss, cubature_transform, stochasticCubatureRulePoints) from ...types.array import StateVector, StateVectors, Matrix, CovarianceMatrix from ...types.state import State, GaussianState @@ -381,4 +381,35 @@ def identity_function(inpu): assert np.allclose(outcovar, instate.covar) assert np.allclose(mean, instate.state_vector) assert np.allclose(covar, instate.covar) +@pytest.mark.parametrize( + "points, order", + [ + (25, 1), + (25, 3), + (25, 5) + ] +) + + +def test_stochastic_integration(): + + #Test 1 + order = 1 + nx = 20 + a, b = stochasticCubatureRulePoints(order, nx) + # Assert some condition which indicates proper functioning + assert np.allclose(a, b) + # Test 2 + order = 3 + nx = 10 + a, b = stochasticCubatureRulePoints(order, nx) + # Assert some condition which indicates proper functioning + assert np.allclose(a, b) + # Test 3 + order = 5 + nx = 10 + a, b = stochasticCubatureRulePoints(order, nx) + assert np.allclose(a, b) + # Assert some condition which indicates proper functioning + assert np.allclose(a, b) From 3a7ecc55fb893539e70e4048074f4d46af00582e Mon Sep 17 00:00:00 2001 From: John Hiles Date: Fri, 26 Jul 2024 23:23:46 -0400 Subject: [PATCH 09/51] Added unit testing for SIF Smoother --- stonesoup/functions/__init__.py | 1 - stonesoup/functions/tests/test_functions.py | 53 +++++++++------------ stonesoup/predictor/kalman.py | 2 +- stonesoup/predictor/tests/test_kalman.py | 2 +- stonesoup/smoother/kalman.py | 2 +- stonesoup/smoother/tests/test_kalman.py | 5 +- stonesoup/updater/kalman.py | 16 +++---- stonesoup/updater/tests/test_kalman.py | 2 +- 8 files changed, 38 insertions(+), 45 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 7e0417191..38617be4d 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1083,4 +1083,3 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. cross_covar = cross_covar.view(CovarianceMatrix) return mean, covar, cross_covar, cubature_points_t - diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index 88f279083..f45c2735e 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -346,6 +346,7 @@ def test_gm_sample(means, covars, weights, size): else: assert samples.shape[0] == means.shape[0] + @pytest.mark.parametrize( "mean, covar, alp", [ @@ -357,8 +358,6 @@ def test_gm_sample(means, covars, weights, size): [0, 0.06, -0.01, 1.1]]), 0.7) ] ) - - def test_cubature_transform(mean, covar, alp): instate = GaussianState(mean, covar) @@ -381,35 +380,29 @@ def identity_function(inpu): assert np.allclose(outcovar, instate.covar) assert np.allclose(mean, instate.state_vector) assert np.allclose(covar, instate.covar) + + @pytest.mark.parametrize( - "points, order", + "order, nx", [ - (25, 1), - (25, 3), - (25, 5) + (3, 3), + (5, 4), + (1, 2) ] ) - - -def test_stochastic_integration(): - - #Test 1 - order = 1 - nx = 20 - a, b = stochasticCubatureRulePoints(order, nx) - # Assert some condition which indicates proper functioning - assert np.allclose(a, b) - # Test 2 - order = 3 - nx = 10 - a, b = stochasticCubatureRulePoints(order, nx) - # Assert some condition which indicates proper functioning - assert np.allclose(a, b) - # Test 3 - order = 5 - nx = 10 - a, b = stochasticCubatureRulePoints(order, nx) - assert np.allclose(a, b) - # Assert some condition which indicates proper functioning - assert np.allclose(a, b) - +def test_stochastic_integration(order, nx): + points, weights = stochasticCubatureRulePoints(nx, order) + # Mean + assert np.allclose(np.average(points, weights=weights, axis=1), + 0, atol=1e-5) + # Weights + assert np.isclose(np.sum(weights), 1, atol=1e-5) + if order != 1: # For order 1 it does not make sense to check variance of points + # Covariance + var = ((weights * points) @ points.T) + # Check if diagonal elements are close to 1 + diagonal_elements = np.diag(var) + assert np.allclose(diagonal_elements, 1, atol=1e-5) + # Check if off-diagonal elements are close to 0 + off_diagonal_elements = var[~np.eye(nx, dtype=bool)] + assert np.allclose(off_diagonal_elements, 0, atol=1e-5) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 78abf1f97..088ea018e 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -747,4 +747,4 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): Pp.view(CovarianceMatrix), timestamp=timestamp, transition_model=self.transition_model, - prior=prior) \ No newline at end of file + prior=prior) diff --git a/stonesoup/predictor/tests/test_kalman.py b/stonesoup/predictor/tests/test_kalman.py index c7aab5a80..24bbf3ef4 100644 --- a/stonesoup/predictor/tests/test_kalman.py +++ b/stonesoup/predictor/tests/test_kalman.py @@ -149,4 +149,4 @@ def test_sqrt_kalman(): sqrt_prediction.sqrt_covar@sqrt_prediction.sqrt_covar.T, 0, atol=1.e-14) assert np.allclose(prediction.covar, sqrt_prediction.covar, 0, atol=1.e-14) - assert prediction.timestamp == sqrt_prediction.timestamp \ No newline at end of file + assert prediction.timestamp == sqrt_prediction.timestamp diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 10e62661d..d6d42b955 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -308,7 +308,7 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): return cross_covar @ np.linalg.inv(prediction.covar) -class SIFKalmanSmoother(KalmanSmoother): +class StochasticIntegrationSmoother(KalmanSmoother): r""" The stochastic integration version of the Kalman filter. As with the parent version of the Kalman smoother, diff --git a/stonesoup/smoother/tests/test_kalman.py b/stonesoup/smoother/tests/test_kalman.py index d6b58ee86..c502b5f2d 100644 --- a/stonesoup/smoother/tests/test_kalman.py +++ b/stonesoup/smoother/tests/test_kalman.py @@ -17,7 +17,7 @@ from stonesoup.types.update import GaussianStateUpdate from stonesoup.updater.kalman import KalmanUpdater from stonesoup.smoother.kalman import KalmanSmoother, ExtendedKalmanSmoother, \ - UnscentedKalmanSmoother + UnscentedKalmanSmoother, StochasticIntegrationSmoother @pytest.fixture( @@ -25,8 +25,9 @@ KalmanSmoother, # Standard Kalman ExtendedKalmanSmoother, # Extended Kalman UnscentedKalmanSmoother, # Unscented Kalman + StochasticIntegrationSmoother # Stochastic Integration Smoother ], - ids=["standard", "extended", "unscented"] + ids=["standard", "extended", "unscented", "sif"] ) def smoother_class(request): return request.param diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index e03144f6e..e42001c95 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -13,7 +13,8 @@ from ..models.base import LinearModel from ..models.measurement.linear import LinearGaussian from ..models.measurement import MeasurementModel -from ..functions import gauss2sigma, unscented_transform, cubature_transform, stochasticCubatureRulePoints +from ..functions import (gauss2sigma, unscented_transform, cubature_transform, + stochasticCubatureRulePoints) from ..measures import Measure, Euclidean @@ -1046,14 +1047,13 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme Pzp = IPz if measurement_noise: - Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) else: - Pzp = Pzp + np.diag(Vz.ravel()) - Pzp = Pzp.astype(np.float64) + Pzp = Pzp + np.diag(Vz.ravel()) + Pzp = Pzp.astype(np.float64) + Pxzp = IPxz - Pxzp = IPxz - - cross_covar = Pxzp.view(CovarianceMatrix) + cross_covar = Pxzp.view(CovarianceMatrix) return MeasurementPrediction.from_state( predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), - cross_covar=cross_covar) \ No newline at end of file + cross_covar=cross_covar) diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index 836d8da5e..19c45d023 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -113,6 +113,7 @@ def test_kalman(updater_class, use_joseph_cov): assert np.array_equal(posterior.hypothesis.measurement, measurement) assert posterior.timestamp == prediction.timestamp + def test_sqrt_kalman(): measurement_model = LinearGaussian(ndim_state=2, mapping=[0], noise_covar=np.array([[0.04]])) @@ -230,4 +231,3 @@ def test_schmidtkalman(): assert np.allclose(update.mean, sk_update.mean) assert np.allclose(update.covar, sk_update.covar) - From a20ade4d8ba3fb67586c7e198ef3026988c1f9ab Mon Sep 17 00:00:00 2001 From: John Hiles Date: Sat, 27 Jul 2024 00:35:08 -0400 Subject: [PATCH 10/51] Fixed unit test with SIF updater --- stonesoup/updater/kalman.py | 1 - stonesoup/updater/tests/test_kalman.py | 3 ++- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index e26699b12..e42001c95 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -934,7 +934,6 @@ class StochasticIntegrationUpdater(KalmanUpdater): doc="order of SIR (orders 1, 3, 5 are currently supported)") @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs): """SIF. diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index b8ac6f5e6..3b2c8870e 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -13,7 +13,8 @@ SqrtKalmanUpdater, IteratedKalmanUpdater, SchmidtKalmanUpdater, - CubatureKalmanUpdater) + CubatureKalmanUpdater, + StochasticIntegrationUpdater) @pytest.fixture(params=[KalmanUpdater, ExtendedKalmanUpdater, UnscentedKalmanUpdater, From 3b28635c27323a17da294a3f962c0dfeacd30cd1 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:09:06 -0400 Subject: [PATCH 11/51] Update stonesoup/predictor/kalman.py Changed line in SIF Updater to use matrix broadcasting. Co-authored-by: Steven Hiscocks --- stonesoup/predictor/kalman.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 088ea018e..90758335b 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -673,8 +673,7 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + \ - npm.repmat(prior.mean, 1, np.size(SCRSigmaPoints, 1)) + xpoints = Sp@SCRSigmaPoints + prior.mean # -- points transformation via dynamics (deterministic part) # Put these points into s State object list sigma_points_states = [] From a68766bc5a44457b6f4738b19dfc0151eeb8a703 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:10:31 -0400 Subject: [PATCH 12/51] Update stonesoup/updater/kalman.py Simplified check on SIF updater and added in place operation for speed. Co-authored-by: Steven Hiscocks --- stonesoup/updater/kalman.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index e42001c95..3de60e1e6 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1008,10 +1008,9 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme zp = Iz N = 0 - while N < self.Nmin or np.all([N < self.Nmax, - np.any([(np.linalg.norm(VPz) > self.Eps), - (np.linalg.norm(VPxz) > self.Eps)])]): - N = N + 1 + while N < self.Nmin or (N < self.Nmax and (np.linalg.norm(VPz) > self.Eps + or np.linalg.norm(VPxz) > self.Eps)): + N += 1 # -- cubature points and weights computation (for standard normal PDF) [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, self.SIorder) From d0da29ed8193cff0b5843b29fc930dae59824270 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:13:53 -0400 Subject: [PATCH 13/51] Update stonesoup/updater/kalman.py Used matrix broadcasting in SIF Updater in place of depriciated np.matrix operation. Co-authored-by: Steven Hiscocks --- stonesoup/updater/kalman.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 3de60e1e6..d2fc09841 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1012,12 +1012,10 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme or np.linalg.norm(VPxz) > self.Eps)): N += 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, - self.SIorder) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + \ - np.matlib.repmat(epMean, 1, np.size(SCRSigmaPoints, 1)) + xpoints = Sp@SCRSigmaPoints + epMean # Points transformation via measurement equation (deterministic part) sigma_points_states = [] for xpoint in xpoints.T: From 60bd029176432c539381547bd9a96a6370af5652 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:14:31 -0400 Subject: [PATCH 14/51] Update stonesoup/updater/kalman.py Converted SIF Updater to inplace operations for speed. Co-authored-by: Steven Hiscocks --- stonesoup/updater/kalman.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index d2fc09841..3dffcdc0d 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1035,12 +1035,12 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme # Update covariance matrix IPz DPz = (SumRPz - IPz) / N - IPz = IPz + DPz - VPz = (N - 2) * VPz / N + DPz ** 2 + IPz += DPz + VPz = (N - 2)*VPz/N + DPz**2 # Update cross-covariance matrix IPxz DPxz = (SumRPxz - IPxz) / N - IPxz = IPxz + DPxz - VPxz = (N - 2) * VPxz / N + DPxz ** 2 + IPxz += DPxz + VPxz = (N - 2)*VPxz/N + DPxz**2 Pzp = IPz if measurement_noise: From e417603ae11db8a54bbdb82604843d4948cf0d4f Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:15:23 -0400 Subject: [PATCH 15/51] Update stonesoup/updater/kalman.py Removed unneccisary parenthesis from SIF Updater Co-authored-by: Steven Hiscocks --- stonesoup/updater/kalman.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 3dffcdc0d..06bf00f09 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1029,9 +1029,8 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme # Matrix and predictive state and measurement covariance matrix hpoints_diff = hpoints - zp - SumRPz = hpoints_diff @ np.diag(w) @ (hpoints_diff.T) - SumRPxz = ((xpoints - xpoints[:, 0:1]) @ np.diag(w) @ - (hpoints_diff).T) + SumRPz = hpoints_diff @ np.diag(w) @ hpoints_diff.T + SumRPxz = (xpoints - xpoints[:, 0:1]) @ np.diag(w) @ hpoints_diff.T # Update covariance matrix IPz DPz = (SumRPz - IPz) / N From 84d91a3220f16d1f2014c7d14a4f44b91b4e2f4b Mon Sep 17 00:00:00 2001 From: pesslovany <43780444+pesslovany@users.noreply.github.com> Date: Tue, 6 Aug 2024 16:26:05 +0200 Subject: [PATCH 16/51] In RandOrthMath changed rand to randn --- stonesoup/functions/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 38617be4d..fc659c8a7 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -828,7 +828,7 @@ def RandOrthMat(n, tol=1e-6): M = np.zeros((n, n)) # Gram-schmidt on random column vectors - vi = np.random.rand(n, 1) + vi = np.random.randn(n, 1) # the n-dimensional normal distribution has spherical symmetry, which implies # that after normalization the drawn vectors would be uniformly distributed on the # n-dimensional unit sphere. @@ -837,7 +837,7 @@ def RandOrthMat(n, tol=1e-6): for i in range(1, n): nrm = 0 while nrm < tol: - vi = np.random.rand(n, 1) + vi = np.random.randn(n, 1) vi = vi - M[:, 0:i] @ (M[:, 0:i].T @ vi) nrm = np.linalg.norm(vi) M[:, i] = (vi / nrm).T From 07b0b6c3430cb9245587bde34dfa58ad90b126db Mon Sep 17 00:00:00 2001 From: John Hiles Date: Tue, 6 Aug 2024 12:39:21 -0400 Subject: [PATCH 17/51] Fixed SIF updater returning an error on the Cross Covariance.:wq --- stonesoup/updater/kalman.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 06bf00f09..36a14059a 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1034,7 +1034,7 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme # Update covariance matrix IPz DPz = (SumRPz - IPz) / N - IPz += DPz + IPz += DPz.reshape(np.shape(IPz)) VPz = (N - 2)*VPz/N + DPz**2 # Update cross-covariance matrix IPxz DPxz = (SumRPxz - IPxz) / N @@ -1043,13 +1043,14 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme Pzp = IPz if measurement_noise: - Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) else: - Pzp = Pzp + np.diag(Vz.ravel()) - Pzp = Pzp.astype(np.float64) - Pxzp = IPxz + Pzp = Pzp + np.diag(Vz.ravel()) + Pzp = Pzp.astype(np.float64) - cross_covar = Pxzp.view(CovarianceMatrix) + Pxzp = IPxz + + cross_covar = Pxzp.view(CovarianceMatrix) return MeasurementPrediction.from_state( predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), cross_covar=cross_covar) From 793deac9de2a06f2bfaefdc48f6d42f20d003eaf Mon Sep 17 00:00:00 2001 From: pesslovany <43780444+pesslovany@users.noreply.github.com> Date: Tue, 6 Aug 2024 20:57:57 +0200 Subject: [PATCH 18/51] Fixed code based on reviewer comments --- stonesoup/functions/__init__.py | 253 +++++++++---------- stonesoup/predictor/kalman.py | 333 +++++++++++++++---------- stonesoup/smoother/kalman.py | 129 +++++----- stonesoup/updater/kalman.py | 416 +++++++++++++++++++------------- 4 files changed, 664 insertions(+), 467 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index fc659c8a7..c0ccf654d 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1,11 +1,13 @@ """Mathematical functions used within Stone Soup""" + import copy import warnings import numpy as np +from scipy.stats import ortho_group +from ..types.array import CovarianceMatrix, StateVector, StateVectors from ..types.numeric import Probability -from ..types.array import StateVector, StateVectors, CovarianceMatrix from ..types.state import State @@ -28,9 +30,7 @@ def tria(matrix): _, upper_triangular = np.linalg.qr(matrix.T) lower_triangular = upper_triangular.T - index = [col - for col, val in enumerate(np.diag(lower_triangular)) - if val < 0] + index = [col for col, val in enumerate(np.diag(lower_triangular)) if val < 0] lower_triangular[:, index] *= -1 @@ -61,8 +61,8 @@ def cholesky_eps(A, lower=False): L = np.zeros(A.shape) for i in range(A.shape[0]): for j in range(i): - L[i, j] = (A[i, j] - L[i, :]@L[j, :].T) / L[j, j] - val = A[i, i] - L[i, :]@L[i, :].T + L[i, j] = (A[i, j] - L[i, :] @ L[j, :].T) / L[j, j] + val = A[i, i] - L[i, :] @ L[i, :].T L[i, i] = np.sqrt(val) if val > eps else np.sqrt(eps) if lower: @@ -93,13 +93,16 @@ def jacobian(fun, x, **kwargs): # For numerical reasons the step size needs to large enough. Aim for 1e-8 # relative to spacing between floating point numbers for each dimension - delta = 1e8*np.spacing(x.state_vector.astype(np.float64).ravel()) + delta = 1e8 * np.spacing(x.state_vector.astype(np.float64).ravel()) # But at least 1e-8 # TODO: Is this needed? If not, note special case at zero. delta[delta < 1e-8] = 1e-8 x2 = copy.copy(x) # Create a clone of the input - x2.state_vector = np.tile(x.state_vector, ndim+1) + np.eye(ndim, ndim+1)*delta[:, np.newaxis] + x2.state_vector = ( + np.tile(x.state_vector, ndim + 1) + + np.eye(ndim, ndim + 1) * delta[:, np.newaxis] + ) x2.state_vector = x2.state_vector.view(StateVectors) F = fun(x2, **kwargs) @@ -170,10 +173,12 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None): sigma_points = sigma_points.astype(float) # Can't use in place addition/subtraction as casting issues may arise when mixing float/int - sigma_points[:, 1:(ndim_state + 1)] = \ - sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma*np.sqrt(c) - sigma_points[:, (ndim_state + 1):] = \ - sigma_points[:, (ndim_state + 1):] - sqrt_sigma*np.sqrt(c) + sigma_points[:, 1 : (ndim_state + 1)] = sigma_points[ + :, 1 : (ndim_state + 1) + ] + sqrt_sigma * np.sqrt(c) + sigma_points[:, (ndim_state + 1) :] = sigma_points[ + :, (ndim_state + 1) : + ] - sqrt_sigma * np.sqrt(c) # Put these sigma points into s State object list sigma_points_states = [] @@ -225,8 +230,14 @@ def sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None): return mean.view(StateVector), covar.view(CovarianceMatrix) -def unscented_transform(sigma_points_states, mean_weights, covar_weights, - fun, points_noise=None, covar_noise=None): +def unscented_transform( + sigma_points_states, + mean_weights, + covar_weights, + fun, + points_noise=None, + covar_noise=None, +): """ Apply the Unscented Transform to a set of sigma points @@ -269,24 +280,33 @@ def unscented_transform(sigma_points_states, mean_weights, covar_weights, An array containing the transformed sigma point covariance weights """ # Reconstruct the sigma_points matrix - sigma_points = StateVectors([ - sigma_points_state.state_vector for sigma_points_state in sigma_points_states]) + sigma_points = StateVectors( + [sigma_points_state.state_vector for sigma_points_state in sigma_points_states] + ) # Transform points through f if points_noise is None: - sigma_points_t = StateVectors([ - fun(sigma_points_state) for sigma_points_state in sigma_points_states]) + sigma_points_t = StateVectors( + [fun(sigma_points_state) for sigma_points_state in sigma_points_states] + ) else: - sigma_points_t = StateVectors([ - fun(sigma_points_state, points_noise) - for sigma_points_state, point_noise in zip(sigma_points_states, points_noise.T)]) + sigma_points_t = StateVectors( + [ + fun(sigma_points_state, points_noise) + for sigma_points_state, point_noise in zip( + sigma_points_states, points_noise.T + ) + ] + ) # Calculate mean and covariance approximation mean, covar = sigma2gauss(sigma_points_t, mean_weights, covar_weights, covar_noise) # Calculate cross-covariance cross_covar = ( - (sigma_points-sigma_points[:, 0:1]) @ np.diag(covar_weights) @ (sigma_points_t-mean).T + (sigma_points - sigma_points[:, 0:1]) + @ np.diag(covar_weights) + @ (sigma_points_t - mean).T ).view(CovarianceMatrix) return mean, covar, cross_covar, sigma_points_t, mean_weights, covar_weights @@ -450,7 +470,7 @@ def az_el_rg2cart(phi, theta, rho): """ x = rho * np.sin(phi) y = rho * np.sin(theta) - z = rho * np.sqrt(1.0 - np.sin(theta)**2 - np.sin(phi)**2) + z = rho * np.sqrt(1.0 - np.sin(theta) ** 2 - np.sin(phi) ** 2) return x, y, z @@ -484,9 +504,7 @@ def rotx(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[one, zero, zero], - [zero, c, -s], - [zero, s, c]]) + return np.array([[one, zero, zero], [zero, c, -s], [zero, s, c]]) def roty(theta): @@ -520,9 +538,7 @@ def roty(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[c, zero, s], - [zero, one, zero], - [-s, zero, c]]) + return np.array([[c, zero, s], [zero, one, zero], [-s, zero, c]]) def rotz(theta): @@ -556,9 +572,7 @@ def rotz(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[c, -s, zero], - [s, c, zero], - [zero, zero, one]]) + return np.array([[c, -s, zero], [s, c, zero], [zero, zero, one]]) def gm_sample(means, covars, size, weights=None): @@ -596,8 +610,12 @@ def gm_sample(means, covars, size, weights=None): weights = np.array([1 / len(means)] * len(means)) n_samples = np.random.multinomial(size, weights) - samples = np.vstack([np.random.multivariate_normal(mean.ravel(), covar, sample) - for (mean, covar, sample) in zip(means, covars, n_samples)]).T + samples = np.vstack( + [ + np.random.multivariate_normal(mean.ravel(), covar, sample) + for (mean, covar, sample) in zip(means, covars, n_samples) + ] + ).T return StateVectors(samples) @@ -632,7 +650,10 @@ def gm_reduce_single(means, covars, weights): # Calculate covar delta_means = means - mean - covar = np.sum(covars*weights, axis=2, dtype=np.float64) + weights*delta_means@delta_means.T + covar = ( + np.sum(covars * weights, axis=2, dtype=np.float64) + + weights * delta_means @ delta_means.T + ) return mean.view(StateVector), covar.view(CovarianceMatrix) @@ -652,7 +673,7 @@ def mod_bearing(x): Angle in radians in the range math: :math:`-\pi` to :math:`+\pi` """ - x = (x+np.pi) % (2.0*np.pi)-np.pi + x = (x + np.pi) % (2.0 * np.pi) - np.pi return x @@ -671,7 +692,7 @@ def mod_elevation(x): float Angle in radians in the range math: :math:`-\pi/2` to :math:`+\pi/2` """ - x = x % (2*np.pi) # limit to 2*pi + x = x % (2 * np.pi) # limit to 2*pi N = x // (np.pi / 2) # Count # of 90 deg multiples if N == 1: x = np.pi - x @@ -759,7 +780,9 @@ def dotproduct(a, b): """ if np.shape(a) != np.shape(b): - raise ValueError("Inputs must be (a collection of) column vectors of the same dimension") + raise ValueError( + "Inputs must be (a collection of) column vectors of the same dimension" + ) # Decide whether this is a StateVector or a StateVectors if type(a) is StateVector and type(b) is StateVector: @@ -767,7 +790,9 @@ def dotproduct(a, b): elif type(a) is StateVectors and type(b) is StateVectors: return np.atleast_2d(np.asarray(np.sum(a * b, axis=0))) else: - raise ValueError("Inputs must be `StateVector` or `StateVectors` and of the same type") + raise ValueError( + "Inputs must be `StateVector` or `StateVectors` and of the same type" + ) def sde_euler_maruyama_integration(fun, t_values, state_x0): @@ -795,56 +820,10 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0): delta_t = next_t - t delta_w = np.random.normal(scale=np.sqrt(delta_t), size=(state_x.ndim, 1)) a, b = fun(state_x, t) - state_x.state_vector = state_x.state_vector + a*delta_t + b@delta_w + state_x.state_vector = state_x.state_vector + a * delta_t + b @ delta_w return state_x.state_vector -def RandOrthMat(n, tol=1e-6): - """Random orthogonal real matrix - - M = RANDORTHMAT(n) - generates a random n x n orthogonal real matrix. - M = RANDORTHMAT(n,tol) - explicitly specifies a thresh value that measures linear dependence - of a newly formed column with the existing columns. Defaults to 1e-6. - - In this version the generated matrix distribution *is* uniform over the - manifold O(n) w.r.t. the induced R^(n^2) Lebesgue measure,at a slight - computational overhead (randn + normalization, as opposed to rand ). - - (c) Ofek Shilon , 2006. - - Parameters - ========== - n integer - matrix size - - Returns - ======= - numpy.ndarray n x n - Random orthogonal real matrix - """ - - M = np.zeros((n, n)) - - # Gram-schmidt on random column vectors - vi = np.random.randn(n, 1) - # the n-dimensional normal distribution has spherical symmetry, which implies - # that after normalization the drawn vectors would be uniformly distributed on the - # n-dimensional unit sphere. - M[:, 0] = (vi * 1 / (np.linalg.norm(vi))).T - - for i in range(1, n): - nrm = 0 - while nrm < tol: - vi = np.random.randn(n, 1) - vi = vi - M[:, 0:i] @ (M[:, 0:i].T @ vi) - nrm = np.linalg.norm(vi) - M[:, i] = (vi / nrm).T - - return (M) - - def stochasticCubatureRulePoints(nx, order): """Stochstic cubature rule points @@ -864,17 +843,17 @@ def stochasticCubatureRulePoints(nx, order): if order == 1: X = np.random.randn(nx, 1) SCRSigmaPoints = np.concatenate((X, -X), axis=1) - weights = (np.array([0.5, 0.5])) - if order == 3: - CRSigmaPoints = np.concatenate((np.zeros((nx, 1)), - np.eye(nx), -np.eye(nx)), axis=1) + weights = np.array([0.5, 0.5]) + elif order == 3: + CRSigmaPoints = np.concatenate( + (np.zeros((nx, 1)), np.eye(nx), -np.eye(nx)), axis=1 + ) rho = np.sqrt(np.random.chisquare(nx + 2)) - Q = RandOrthMat(nx) + Q = ortho_group.rvs(nx) SCRSigmaPoints = Q * rho @ CRSigmaPoints - weights = np.insert(0.5 * np.ones(2 * nx) / rho ** 2, 0, - (1 - nx / rho ** 2)) + weights = np.insert(0.5 * np.ones(2 * nx) / rho**2, 0, (1 - nx / rho**2)) - if order == 5: + elif order == 5: # generating random values r = np.sqrt(np.random.chisquare(2 * nx + 7)) @@ -884,46 +863,62 @@ def stochasticCubatureRulePoints(nx, order): delta = r * np.cos(np.arcsin(q) / 2) # calculating weights - c1up = nx + 2 - delta ** 2 - c1do = rho ** 2 * (rho ** 2 - delta ** 2) - c2up = nx + 2 - rho ** 2 - c2do = delta ** 2 * (delta ** 2 - rho ** 2) + c1up = nx + 2 - delta**2 + c1do = rho**2 * (rho**2 - delta**2) + c2up = nx + 2 - rho**2 + c2do = delta**2 * (delta**2 - rho**2) cdo = 2 * (nx + 1) ** 2 * (nx + 2) - c3 = (7 - nx) * nx ** 2 + c3 = (7 - nx) * nx**2 c4 = 4 * (nx - 1) ** 2 coef1 = c1up * c3 / cdo / c1do coef2 = c2up * c3 / cdo / c2do coef3 = c1up * c4 / cdo / c1do coef4 = c2up * c4 / cdo / c2do - pom = np.concatenate((np.ones(2 * nx + 2) * coef1, - np.ones(2 * nx + 2) * coef2, - np.ones(nx * (nx + 1)) * coef3, - np.ones(nx * (nx + 1)) * coef4), - axis=0) - weights = np.insert(pom, 0, - (1 - nx * (rho ** 2 + delta ** 2 - nx - 2) / - (rho ** 2 * delta ** 2))) + pom = np.concatenate( + ( + np.ones(2 * nx + 2) * coef1, + np.ones(2 * nx + 2) * coef2, + np.ones(nx * (nx + 1)) * coef3, + np.ones(nx * (nx + 1)) * coef4, + ), + axis=0, + ) + weights = np.insert( + pom, 0, (1 - nx * (rho**2 + delta**2 - nx - 2) / (rho**2 * delta**2)) + ) # Calculating sigma points - Q = RandOrthMat(nx) + Q = ortho_group.rvs(nx) v = np.zeros((nx, nx + 1)) for i in range(0, nx): - v[i, i] = np.sqrt((nx + 1)*(nx - i) / nx / (nx - i + 1)) + v[i, i] = np.sqrt((nx + 1) * (nx - i) / nx / (nx - i + 1)) for j in range(i + 1, nx + 1): v[i, j] = -np.sqrt((nx + 1) / ((nx - i) * nx * (nx - i + 1))) v = Q @ v - y = np.zeros((nx, int(nx*(nx + 1) / 2))) + y = np.zeros((nx, int(nx * (nx + 1) / 2))) cnt = -1 for j in range(0, nx + 1): for i in range(0, j): cnt = cnt + 1 - y[:, cnt] = (v[:, j] + v[:, i]) / np.linalg.norm( - v[:, j] + v[:, i], 2) + y[:, cnt] = (v[:, j] + v[:, i]) / np.linalg.norm(v[:, j] + v[:, i], 2) + + SCRSigmaPoints = np.block( + [ + np.zeros((nx, 1)), + -rho * v, + rho * v, + -delta * v, + +delta * v, + -rho * y, + rho * y, + -delta * y, + delta * y, + ] + ) + else: + raise ValueError("This order of SIF is not supported") - SCRSigmaPoints = np.block([np.zeros((nx, 1)), - -rho*v, rho*v, -delta*v, +delta*v, -rho*y, - rho*y, -delta*y, delta*y]) return (SCRSigmaPoints, weights) @@ -961,13 +956,14 @@ def gauss2cubature(state, alpha=1.0): ndim_state = np.shape(state.state_vector)[0] sqrt_covar = np.linalg.cholesky(state.covar) - cuba_points = np.sqrt(alpha*ndim_state) * np.hstack((np.identity(ndim_state), - -np.identity(ndim_state))) + cuba_points = np.sqrt(alpha * ndim_state) * np.hstack( + (np.identity(ndim_state), -np.identity(ndim_state)) + ) if np.issubdtype(cuba_points.dtype, np.integer): cuba_points = cuba_points.astype(float) - cuba_points = sqrt_covar@cuba_points + state.mean + cuba_points = sqrt_covar @ cuba_points + state.mean return StateVectors(cuba_points) @@ -1012,7 +1008,7 @@ def cubature2gauss(cubature_points, covar_noise=None, alpha=1.0): mean = np.average(cubature_points, axis=1) sigma_mult = cubature_points @ cubature_points.T mean_mult = mean @ mean.T - covar = (1/alpha)*((1/m)*sigma_mult - mean_mult) + covar = (1 / alpha) * ((1 / m) * sigma_mult - mean_mult) if covar_noise is not None: covar = covar + covar_noise @@ -1070,16 +1066,23 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. cubature_points = gauss2cubature(state) if points_noise is None: - cubature_points_t = StateVectors([fun(State(cub_point)) for cub_point in cubature_points]) + cubature_points_t = StateVectors( + [fun(State(cub_point)) for cub_point in cubature_points] + ) else: - cubature_points_t = StateVectors([ - fun(State(cub_point), points_noise) - for cub_point, point_noise in zip(cubature_points, points_noise)]) + cubature_points_t = StateVectors( + [ + fun(State(cub_point), points_noise) + for cub_point, point_noise in zip(cubature_points, points_noise) + ] + ) mean, covar = cubature2gauss(cubature_points_t, covar_noise) - cross_covar = (1/alpha)*((1./(2*ndim_state))*cubature_points@cubature_points_t.T - - np.average(cubature_points, axis=1)@mean.T) + cross_covar = (1 / alpha) * ( + (1.0 / (2 * ndim_state)) * cubature_points @ cubature_points_t.T + - np.average(cubature_points, axis=1) @ mean.T + ) cross_covar = cross_covar.view(CovarianceMatrix) return mean, covar, cross_covar, cubature_points_t diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 90758335b..022916be7 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -1,22 +1,21 @@ +import copy from functools import partial import numpy as np -import numpy.matlib as npm import scipy.linalg as la -import copy -from ..types.array import CovarianceMatrix, StateVector, StateVectors -from .base import Predictor -from ._utils import predict_lru_cache from ..base import Property -from ..types.prediction import Prediction, SqrtGaussianStatePrediction +from ..functions import (cubature_transform, gauss2sigma, + stochasticCubatureRulePoints, unscented_transform) from ..models.base import LinearModel -from ..models.transition import TransitionModel -from ..models.transition.linear import LinearGaussianTransitionModel from ..models.control import ControlModel from ..models.control.linear import LinearControlModel -from ..functions import gauss2sigma, unscented_transform, cubature_transform, \ - stochasticCubatureRulePoints +from ..models.transition import TransitionModel +from ..models.transition.linear import LinearGaussianTransitionModel +from ..types.array import CovarianceMatrix, StateVector, StateVectors +from ..types.prediction import Prediction, SqrtGaussianStatePrediction +from ._utils import predict_lru_cache +from .base import Predictor class KalmanPredictor(Predictor): @@ -40,11 +39,13 @@ class KalmanPredictor(Predictor): """ transition_model: LinearGaussianTransitionModel = Property( - doc="The transition model to be used.") + doc="The transition model to be used." + ) control_model: LinearControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.") + "will create a zero-effect linear :class:`~.ControlModel`.", + ) def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -53,8 +54,9 @@ def __init__(self, *args, **kwargs): if self.control_model is None: ndims = self.transition_model.ndim_state ndimc = 2 # No control exerted so this doesn't matter. - self.control_model = LinearControlModel(np.zeros([ndims, ndimc]), - control_noise=np.zeros([ndimc, ndimc])) + self.control_model = LinearControlModel( + np.zeros([ndims, ndimc]), control_noise=np.zeros([ndimc, ndimc]) + ) def _transition_matrix(self, **kwargs): """Return the transition matrix @@ -129,7 +131,9 @@ def _predict_over_interval(self, prior, timestamp): return predict_over_interval - def _predicted_covariance(self, prior, predict_over_interval, control_input=None, **kwargs): + def _predicted_covariance( + self, prior, predict_over_interval, control_input=None, **kwargs + ): """Private function to return the predicted covariance. Useful in that it can be overwritten in children. @@ -149,17 +153,25 @@ def _predicted_covariance(self, prior, predict_over_interval, control_input=None """ prior_cov = prior.covar - trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, - **kwargs) - trans_cov = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) + trans_m = self._transition_matrix( + prior=prior, time_interval=predict_over_interval, **kwargs + ) + trans_cov = self.transition_model.covar( + time_interval=predict_over_interval, **kwargs + ) # As this is Kalman-like, the control model must be capable of # returning a control matrix (B) - ctrl_mat = self._control_matrix(control_input=control_input, - time_interval=predict_over_interval, **kwargs) + ctrl_mat = self._control_matrix( + control_input=control_input, time_interval=predict_over_interval, **kwargs + ) ctrl_noi = self.control_model.control_noise - return trans_m @ prior_cov @ trans_m.T + trans_cov + ctrl_mat @ ctrl_noi @ ctrl_mat.T + return ( + trans_m @ prior_cov @ trans_m.T + + trans_cov + + ctrl_mat @ ctrl_noi @ ctrl_mat.T + ) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -191,18 +203,25 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # Prediction of the mean x_pred = self._transition_function( - prior, time_interval=predict_over_interval, **kwargs) \ - + self.control_model.function(control_input, time_interval=predict_over_interval, - **kwargs) + prior, time_interval=predict_over_interval, **kwargs + ) + self.control_model.function( + control_input, time_interval=predict_over_interval, **kwargs + ) # Prediction of the covariance - p_pred = self._predicted_covariance(prior, predict_over_interval, - control_input=control_input, - **kwargs) + p_pred = self._predicted_covariance( + prior, predict_over_interval, control_input=control_input, **kwargs + ) # And return the state in the correct form - return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, - transition_model=self.transition_model, prior=prior) + return Prediction.from_state( + prior, + x_pred, + p_pred, + timestamp=timestamp, + transition_model=self.transition_model, + prior=prior, + ) class ExtendedKalmanPredictor(KalmanPredictor): @@ -223,7 +242,8 @@ class ExtendedKalmanPredictor(KalmanPredictor): control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.") + "will create a zero-effect linear :class:`~.ControlModel`.", + ) def _transition_matrix(self, prior, linearisation_point=None, **kwargs): r"""Returns the transition matrix, a matrix if the model is linear, or @@ -297,23 +317,26 @@ class UnscentedKalmanPredictor(KalmanPredictor): Gaussian mean and covariance, then putting these through the (in general non-linear) transition function, then reconstructing the Gaussian. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.") + "will create a zero-effect linear :class:`~.ControlModel`.", + ) alpha: float = Property( - default=0.5, - doc="Primary sigma point spread scaling parameter. Default is 0.5.") + default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5." + ) beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " - "true distribution is Gaussian, the value of 2 is optimal. " - "Default is 2") + "true distribution is Gaussian, the value of 2 is optimal. " + "Default is 2", + ) kappa: float = Property( default=None, - doc="Secondary spread scaling parameter. Default is calculated as " - "3-Ns") + doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns", + ) def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -338,8 +361,9 @@ def _transition_and_control_function(self, prior_state, **kwargs): control """ - return self.transition_model.function(prior_state, **kwargs) + \ - self.control_model.function(**kwargs) + return self.transition_model.function( + prior_state, **kwargs + ) + self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -371,33 +395,47 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # TODO: covariances, i.e. sum them before calculating # TODO: the sigma points and then just sticking them into the # TODO: unscented transform, and I haven't checked the statistics. - ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) + ctrl_mat = self.control_model.matrix( + time_interval=predict_over_interval, **kwargs + ) ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = \ - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ + total_noise_covar = ( + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) + ctrl_mat @ ctrl_noi @ ctrl_mat.T + ) # Get the sigma points from the prior mean and covariance. sigma_point_states, mean_weights, covar_weights = gauss2sigma( - prior, self.alpha, self.beta, self.kappa) + prior, self.alpha, self.beta, self.kappa + ) # This ensures that function passed to unscented transform has the # correct time interval transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, - time_interval=predict_over_interval) + time_interval=predict_over_interval, + ) # Put these through the unscented transform, together with the total # covariance to get the parameters of the Gaussian x_pred, p_pred, _, _, _, _ = unscented_transform( - sigma_point_states, mean_weights, covar_weights, - transition_and_control_function, covar_noise=total_noise_covar + sigma_point_states, + mean_weights, + covar_weights, + transition_and_control_function, + covar_noise=total_noise_covar, ) # and return a Gaussian state based on these parameters - return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, - transition_model=self.transition_model, prior=prior) + return Prediction.from_state( + prior, + x_pred, + p_pred, + timestamp=timestamp, + transition_model=self.transition_model, + prior=prior, + ) class SqrtKalmanPredictor(ExtendedKalmanPredictor): @@ -422,15 +460,19 @@ class SqrtKalmanPredictor(ExtendedKalmanPredictor): Springfield, VA. """ + qr_method: bool = Property( default=False, doc="A switch to do the prediction via a QR decomposition, rather than using a Cholesky " - "decomposition.") + "decomposition.", + ) # This predictor returns a square root form of the Gaussian state prediction _prediction_class = SqrtGaussianStatePrediction - def _predicted_covariance(self, prior, predict_over_interval, control_input=None, **kwargs): + def _predicted_covariance( + self, prior, predict_over_interval, control_input=None, **kwargs + ): """Private function to return the predicted covariance. Parameters @@ -450,36 +492,47 @@ def _predicted_covariance(self, prior, predict_over_interval, control_input=None """ sqrt_prior_cov = prior.sqrt_covar - trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, - **kwargs) + trans_m = self._transition_matrix( + prior=prior, time_interval=predict_over_interval, **kwargs + ) try: - sqrt_trans_cov = self.transition_model.sqrt_covar(time_interval=predict_over_interval, - **kwargs) + sqrt_trans_cov = self.transition_model.sqrt_covar( + time_interval=predict_over_interval, **kwargs + ) except AttributeError: - sqrt_trans_cov = la.sqrtm(self.transition_model.covar( - time_interval=predict_over_interval, **kwargs)) + sqrt_trans_cov = la.sqrtm( + self.transition_model.covar( + time_interval=predict_over_interval, **kwargs + ) + ) # As this is Kalman-like, the control model must be capable of returning a control matrix # (B) - ctrl_mat = self._control_matrix(control_input=control_input, - time_interval=predict_over_interval) + ctrl_mat = self._control_matrix( + control_input=control_input, time_interval=predict_over_interval + ) try: - sqrt_ctrl_noi = self.control_model.sqrt_covar(time_interval=predict_over_interval, - **kwargs) + sqrt_ctrl_noi = self.control_model.sqrt_covar( + time_interval=predict_over_interval, **kwargs + ) except AttributeError: - sqrt_ctrl_noi = la.sqrtm(self.control_model.covar(time_interval=predict_over_interval, - **kwargs)) + sqrt_ctrl_noi = la.sqrtm( + self.control_model.covar(time_interval=predict_over_interval, **kwargs) + ) if self.qr_method: # Note that the control matrix aspect of this hasn't been tested - m_sq_trans_cov = np.block([[trans_m @ sqrt_prior_cov, sqrt_trans_cov, - ctrl_mat@sqrt_ctrl_noi]]) + m_sq_trans_cov = np.block( + [[trans_m @ sqrt_prior_cov, sqrt_trans_cov, ctrl_mat @ sqrt_ctrl_noi]] + ) _, pred_sqrt_cov = np.linalg.qr(m_sq_trans_cov.T) return pred_sqrt_cov.T else: - return np.linalg.cholesky(trans_m@sqrt_prior_cov@sqrt_prior_cov.T@trans_m.T + - sqrt_trans_cov@sqrt_trans_cov.T + - ctrl_mat@sqrt_ctrl_noi@sqrt_ctrl_noi.T@ctrl_mat.T) + return np.linalg.cholesky( + trans_m @ sqrt_prior_cov @ sqrt_prior_cov.T @ trans_m.T + + sqrt_trans_cov @ sqrt_trans_cov.T + + ctrl_mat @ sqrt_ctrl_noi @ sqrt_ctrl_noi.T @ ctrl_mat.T + ) class CubatureKalmanPredictor(KalmanPredictor): @@ -491,15 +544,18 @@ class CubatureKalmanPredictor(KalmanPredictor): :meth:`cubature_transform` function. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.") + "will create a zero-effect linear :class:`~.ControlModel`.", + ) alpha: float = Property( default=1.0, doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + "vice versa.", + ) def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions @@ -519,8 +575,9 @@ def _transition_and_control_function(self, prior_state, **kwargs): control """ - return self.transition_model.function(prior_state, **kwargs) \ - + self.control_model.function(**kwargs) + return self.transition_model.function( + prior_state, **kwargs + ) + self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -549,24 +606,38 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # The covariance on the transition model + the control model # TODO: See equivalent note to unscented transform. - ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) + ctrl_mat = self.control_model.matrix( + time_interval=predict_over_interval, **kwargs + ) ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = \ - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ - + ctrl_mat@ctrl_noi@ctrl_mat.T + total_noise_covar = ( + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) + + ctrl_mat @ ctrl_noi @ ctrl_mat.T + ) # This ensures that function passed to transform has the correct time interval and control # input transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, - time_interval=predict_over_interval) + time_interval=predict_over_interval, + ) - x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, - covar_noise=total_noise_covar, alpha=self.alpha) + x_pred, p_pred, _, _ = cubature_transform( + prior, + transition_and_control_function, + covar_noise=total_noise_covar, + alpha=self.alpha, + ) - return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, - transition_model=self.transition_model, prior=prior) + return Prediction.from_state( + prior, + x_pred, + p_pred, + timestamp=timestamp, + transition_model=self.transition_model, + prior=prior, + ) class StochasticIntegrationPredictor(KalmanPredictor): @@ -575,23 +646,22 @@ class StochasticIntegrationPredictor(KalmanPredictor): The state prediction of nonlinear models is accomplished by the stochastic integration approximation. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.") - Nmax: float = Property( - default=10, - doc="maximal number of iterations of SIR") + "will create a zero-effect linear :class:`~.ControlModel`.", + ) + Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") Nmin: float = Property( default=5, - doc="minimal number of iterations of stochastic integration rule (SIR)") - Eps: float = Property( - default=5e-3, - doc="allowed threshold for integration error") + doc="minimal number of iterations of stochastic integration rule (SIR)", + ) + Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") SIorder: float = Property( - default=3, - doc="order of SIR (orders 1, 3, 5 are currently supported)") + default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)" + ) def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -616,8 +686,9 @@ def _transition_and_control_function(self, prior_state, **kwargs): control """ - return self.transition_model.function(prior_state, **kwargs) + \ - self.control_model.function(**kwargs) + return self.transition_model.function( + prior_state, **kwargs + ) + self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -660,20 +731,27 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, - time_interval=predict_over_interval) + time_interval=predict_over_interval, + ) # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, - np.any([(np.linalg.norm(Vx) > self.Eps), - ])]): + while N < self.Nmin or np.all( + [ + N < self.Nmax, + np.any( + [ + (np.linalg.norm(Vx) > self.Eps), + ] + ), + ] + ): N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, - self.SIorder) + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + prior.mean + xpoints = Sp @ SCRSigmaPoints + prior.mean # -- points transformation via dynamics (deterministic part) # Put these points into s State object list sigma_points_states = [] @@ -682,9 +760,12 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) - fpoints = StateVectors([ - transition_and_control_function(sigma_points_state) - for sigma_points_state in sigma_points_states]) + fpoints = StateVectors( + [ + transition_and_control_function(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) # Stochastic integration rule for predictive state mean and # covariance matrix SumRx = np.average(fpoints, axis=1, weights=w) @@ -692,24 +773,25 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # --- update mean Ix Dx = (SumRx - Ix) / N Ix = Ix + Dx - Vx = (N - 2) * Vx / N + Dx ** 2 + Vx = (N - 2) * Vx / N + Dx**2 xp = Ix N = 0 # - SIR recursion for state predictive moments computation # -- until max iterations are reached or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, - np.any([(np.linalg.norm(VPx) > - self.Eps)])]): + while N < self.Nmin or np.all( + [N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])] + ): N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, - self.SIorder) - # -- points transformation for given filtering mean and covariance - # matrix - xpoints = Sp @ SCRSigmaPoints + \ - npm.repmat(prior.mean, 1, np.size(SCRSigmaPoints, 1)) + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, self.SIorder) + + # Use np.tile to replicate the mean vector + mean_replicated = np.tile(prior.mean, (1, np.size(SCRSigmaPoints, 1))) + # Matrix multiplication and addition + xpoints = Sp @ SCRSigmaPoints + mean_replicated + # -- points transformation via dynamics (deterministic part) # Put these points into s State object list sigma_points_states = [] @@ -718,32 +800,37 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) - fpoints = StateVectors([ - transition_and_control_function(sigma_points_state) - for sigma_points_state in sigma_points_states]) + fpoints = StateVectors( + [ + transition_and_control_function(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) # -- stochastic integration rule for predictive state mean and covariance # matrix fpoints_diff = fpoints - xp SumRPx = fpoints_diff @ np.diag(w) @ fpoints_diff.T - # SumRPx=np.multiply(pom,fpoints)@fpoints.T # --- update covariance matrix IPx DPx = (SumRPx - IPx) / N IPx = IPx + DPx - VPx = (N - 2) * VPx / N + DPx ** 2 + VPx = (N - 2) * VPx / N + DPx**2 Q = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) Pp = IPx Pp = Pp + Q + np.diag(Vx.ravel()) - Pp = (Pp+Pp.T) / 2 + Pp = (Pp + Pp.T) / 2 Pp = Pp.astype(np.float64) # and return a Gaussian state based on these parameters - return Prediction.from_state(prior, xp.view(StateVector), - Pp.view(CovarianceMatrix), - timestamp=timestamp, - transition_model=self.transition_model, - prior=prior) + return Prediction.from_state( + prior, + xp.view(StateVector), + Pp.view(CovarianceMatrix), + timestamp=timestamp, + transition_model=self.transition_model, + prior=prior, + ) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index d6d42b955..bb94dec09 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -3,16 +3,17 @@ import numpy as np -from .base import Smoother from ..base import Property +from ..functions import (gauss2sigma, stochasticCubatureRulePoints, + unscented_transform) from ..models.base import LinearModel -from ..types.multihypothesis import MultipleHypothesis -from ..types.prediction import GaussianStatePrediction -from ..types.update import GaussianStateUpdate from ..models.transition.base import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel -from ..functions import gauss2sigma, unscented_transform, stochasticCubatureRulePoints from ..types.array import StateVector, StateVectors +from ..types.multihypothesis import MultipleHypothesis +from ..types.prediction import GaussianStatePrediction +from ..types.update import GaussianStateUpdate +from .base import Smoother class KalmanSmoother(Smoother): @@ -56,10 +57,11 @@ class KalmanSmoother(Smoother): transition_model: LinearGaussianTransitionModel = Property( doc="The transition model. The :meth:`smooth` function will initially look for a " - "transition model in the prediction. If that is not found then this one is used.") + "transition model in the prediction. If that is not found then this one is used." + ) def _prediction(self, state): - """ Return the predicted state, either from the prediction directly, or from the attached + """Return the predicted state, either from the prediction directly, or from the attached hypothesis if the queried state is an Update. If not a :class:`~.GaussianStatePrediction` or :class:`~.GaussianStateUpdate` a :class:`~.TypeError` is thrown. @@ -84,14 +86,17 @@ def _prediction(self, state): else: # Multiple predictions, so can't process this. raise ValueError( - "Track has MultipleHypothesis updates with multiple predictions.") + "Track has MultipleHypothesis updates with multiple predictions." + ) else: return state.hypothesis.prediction else: - raise TypeError("States must be GaussianStatePredictions or GaussianStateUpdates.") + raise TypeError( + "States must be GaussianStatePredictions or GaussianStateUpdates." + ) def _transition_model(self, prediction): - """ If it exists, return the transition model from the prediction associated with input + """If it exists, return the transition model from the prediction associated with input state. If that doesn't exist then use the (static) transition model defined by the smoother. @@ -112,7 +117,7 @@ def _transition_model(self, prediction): return transition_model def _transition_matrix(self, state, transition_model, **kwargs): - """ Return the transition matrix + """Return the transition matrix Parameters ---------- @@ -146,9 +151,13 @@ def _smooth_gain(self, state, prediction, **kwargs): The smoothing gain """ - return state.covar \ - @ self._transition_matrix(state, self._transition_model(prediction), **kwargs).T \ + return ( + state.covar + @ self._transition_matrix( + state, self._transition_model(prediction), **kwargs + ).T @ np.linalg.inv(prediction.covar) + ) def smooth(self, track, **kwargs): """ @@ -182,11 +191,15 @@ def smooth(self, track, **kwargs): prediction = self._prediction(subsq_state) # The smoothing gain, mean and covariance ksmooth_gain = self._smooth_gain( - state, prediction, time_interval=time_interval, **kwargs) - smooth_mean = state.state_vector + ksmooth_gain @ (subsq_state.state_vector - - prediction.state_vector) - smooth_covar = state.covar + \ - ksmooth_gain @ (subsq_state.covar - prediction.covar) @ ksmooth_gain.T + state, prediction, time_interval=time_interval, **kwargs + ) + smooth_mean = state.state_vector + ksmooth_gain @ ( + subsq_state.state_vector - prediction.state_vector + ) + smooth_covar = ( + state.covar + + ksmooth_gain @ (subsq_state.covar - prediction.covar) @ ksmooth_gain.T + ) # Create a new type called SmoothedState? @@ -205,7 +218,7 @@ def smooth(self, track, **kwargs): class ExtendedKalmanSmoother(KalmanSmoother): - r""" The extended version of the Kalman smoother. The equations are modified slightly, + r"""The extended version of the Kalman smoother. The equations are modified slightly, analogously to the extended Kalman filter, .. math:: @@ -223,9 +236,12 @@ class ExtendedKalmanSmoother(KalmanSmoother): matrix to calculate the smoothing gain. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") - def _transition_matrix(self, state, transition_model, linearisation_point=None, **kwargs): + def _transition_matrix( + self, state, transition_model, linearisation_point=None, **kwargs + ): r"""Returns the transition matrix, a matrix if the model is linear, or approximated as Jacobian otherwise. Parameters @@ -260,20 +276,22 @@ class UnscentedKalmanSmoother(KalmanSmoother): unscented transform is used to calculate the smoothing gain. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") alpha: float = Property( - default=0.5, - doc="Primary sigma point spread scaling parameter. Default is 0.5.") + default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5." + ) beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " - "true distribution is Gaussian, the value of 2 is optimal. " - "Default is 2") + "true distribution is Gaussian, the value of 2 is optimal. " + "Default is 2", + ) kappa: float = Property( default=0, - doc="Secondary spread scaling parameter. Default is calculated as " - "3-Ns") + doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns", + ) def _smooth_gain(self, state, prediction, time_interval, **kwargs): """Calculate the smoothing gain @@ -293,17 +311,18 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): """ # This ensures that the time interval is correctly applied. transition_function = partial( - self._transition_model(prediction).function, - time_interval=time_interval) + self._transition_model(prediction).function, time_interval=time_interval + ) # Get the sigma points from the mean and covariance. sigma_point_states, mean_weights, covar_weights = gauss2sigma( - state, self.alpha, self.beta, self.kappa) + state, self.alpha, self.beta, self.kappa + ) # Use the unscented transform to return the cross-covariance _, _, cross_covar, _, _, _ = unscented_transform( - sigma_point_states, mean_weights, covar_weights, - transition_function) + sigma_point_states, mean_weights, covar_weights, transition_function + ) return cross_covar @ np.linalg.inv(prediction.covar) @@ -315,20 +334,18 @@ class StochasticIntegrationSmoother(KalmanSmoother): the mean and covariance of the prediction are retrieved from the track. The stochastic integration is used to calculate the smoothing gain. """ + transition_model: TransitionModel = Property(doc="The transition model to be used.") - Nmax: float = Property( - default=10, - doc="maximal number of iterations of SIR") + Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") Nmin: float = Property( default=5, - doc="minimal number of iterations of stochastic integration rule (SIR)") - Eps: float = Property( - default=5e-3, - doc="allowed threshold for integration error") + doc="minimal number of iterations of stochastic integration rule (SIR)", + ) + Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") SIorder: float = Property( - default=3, - doc="order of SIR (orders 1, 3, 5 are currently supported)") + default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)" + ) def _smooth_gain(self, state, prediction, time_interval, **kwargs): """Calculate the smoothing gain @@ -354,8 +371,8 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # This ensures that the time interval is correctly applied. transition_function = partial( - self._transition_model(state).function, - time_interval=time_interval) + self._transition_model(state).function, time_interval=time_interval + ) nx = predMean.shape[0] @@ -368,18 +385,18 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # - SIR recursion for measurement predictive moments computation # -- until either required number of iterations is reached or threshold is reached - while N < self.Nmin or all([N < self.Nmax, - (np.linalg.norm(VPxx) > self.Eps)]): + while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): N += 1 # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, - self.SIorder) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) # -- points transformation for given filtering mean and covariance matrix - # xpoints = np.dot(Sf, SCRSigmaPoints) + efMean[:, np.newaxis] - xpoints = Sf@SCRSigmaPoints + \ - np.matlib.repmat(efMean, 1, np.size(SCRSigmaPoints, 1)) + # Use np.tile to replicate the mean vector + efMeanTiled = np.tile(efMean, (1, np.size(SCRSigmaPoints, 1))) + # Matrix multiplication and addition + xpoints = Sf @ SCRSigmaPoints + efMeanTiled + # -- points transformation via measurement equation (deterministic part) # fpoints = ffunct(xpoints, wMean, Time) @@ -388,21 +405,23 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): state_copy = copy.copy(prediction) state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) - fpoints = StateVectors([ - transition_function(sigma_points_state) - for sigma_points_state in sigma_points_states]) + fpoints = StateVectors( + [ + transition_function(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) # Stochastic integration rule for predictive measurement # mean and covariance matrix fpoints_diff = fpoints - predMean xpoints_diff = xpoints - filtMean - SumRPxx = xpoints_diff @ (fpoints_diff * - np.matlib.repmat(w, nx, 1)).T + SumRPxx = xpoints_diff @ (fpoints_diff * np.matlib.repmat(w, nx, 1)).T # --- update cross-covariance matrix IPxx DPxx = (SumRPxx - IPxx) / N IPxx += DPxx - VPxx = (N - 2) * VPxx / N + DPxx ** 2 + VPxx = (N - 2) * VPxx / N + DPxx**2 # - measurement predictive moments Pxxps = IPxx diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 36a14059a..13c5b420a 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1,21 +1,21 @@ +import copy import warnings +from functools import lru_cache -import copy import numpy as np import scipy.linalg as la -from functools import lru_cache from ..base import Property -from .base import Updater +from ..functions import (cubature_transform, gauss2sigma, + stochasticCubatureRulePoints, unscented_transform) +from ..measures import Euclidean, Measure +from ..models.base import LinearModel +from ..models.measurement import MeasurementModel +from ..models.measurement.linear import LinearGaussian from ..types.array import CovarianceMatrix, StateVector, StateVectors from ..types.prediction import MeasurementPrediction from ..types.update import Update -from ..models.base import LinearModel -from ..models.measurement.linear import LinearGaussian -from ..models.measurement import MeasurementModel -from ..functions import (gauss2sigma, unscented_transform, cubature_transform, - stochasticCubatureRulePoints) -from ..measures import Measure, Euclidean +from .base import Updater class KalmanUpdater(Updater): @@ -65,20 +65,24 @@ class KalmanUpdater(Updater): measurement_model: LinearGaussian = Property( default=None, doc="A linear Gaussian measurement model. This need not be defined if " - "a measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.") + "a measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.", + ) force_symmetric_covariance: bool = Property( default=False, doc="A flag to force the output covariance matrix to be symmetric by way of a simple " - "geometric combination of the matrix and transpose. Default is False.") + "geometric combination of the matrix and transpose. Default is False.", + ) use_joseph_cov: bool = Property( default=False, doc="Bool dictating the method of covariance calculation. If use_joseph_cov is True then " - "the Joseph form of the covariance equation is used.") + "the Joseph form of the covariance equation is used.", + ) - def _measurement_matrix(self, predicted_state=None, measurement_model=None, - **kwargs): + def _measurement_matrix( + self, predicted_state=None, measurement_model=None, **kwargs + ): r"""This is straightforward Kalman so just get the Matrix from the measurement model. @@ -98,8 +102,7 @@ def _measurement_matrix(self, predicted_state=None, measurement_model=None, The measurement matrix, :math:`H_k` """ - return self._check_measurement_model( - measurement_model).matrix(**kwargs) + return self._check_measurement_model(measurement_model).matrix(**kwargs) def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ @@ -121,7 +124,9 @@ def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ return predicted_state.covar @ measurement_matrix.T - def _innovation_covariance(self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs): + def _innovation_covariance( + self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs + ): """Compute the innovation covariance Parameters @@ -146,7 +151,9 @@ def _innovation_covariance(self, m_cross_cov, meas_mat, meas_mod, measurement_no innov_covar += meas_mod.covar(**kwargs) return innov_covar - def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement_prediction): + def _posterior_mean( + self, predicted_state, kalman_gain, measurement, measurement_prediction + ): r"""Compute the posterior mean, :math:`\mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + K_k \mathbf{y}_k`, where the innovation :math:`\mathbf{y}_k = \mathbf{z}_k - h(\mathbf{x}_{k|k-1}). @@ -167,8 +174,9 @@ def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement : :class:`StateVector` The posterior mean estimate """ - post_mean = predicted_state.state_vector + \ - kalman_gain @ (measurement.state_vector - measurement_prediction.state_vector) + post_mean = predicted_state.state_vector + kalman_gain @ ( + measurement.state_vector - measurement_prediction.state_vector + ) return post_mean.view(StateVector) def _posterior_covariance(self, hypothesis): @@ -196,15 +204,18 @@ def _posterior_covariance(self, hypothesis): id_matrix = np.identity(hypothesis.prediction.ndim) # Calculate Kalman gain - kalman_gain = hypothesis.measurement_prediction.cross_covar @ \ - np.linalg.inv(hypothesis.measurement_prediction.covar) + kalman_gain = hypothesis.measurement_prediction.cross_covar @ np.linalg.inv( + hypothesis.measurement_prediction.covar + ) measurement_model = self._check_measurement_model( - hypothesis.measurement.measurement_model) + hypothesis.measurement.measurement_model + ) # Calculate measurement matrix/jacobian matrix - meas_matrix = self._measurement_matrix(hypothesis.prediction, - measurement_model) + meas_matrix = self._measurement_matrix( + hypothesis.prediction, measurement_model + ) # Calculate Prior covariance prior_covar = hypothesis.prediction.covar @@ -214,21 +225,26 @@ def _posterior_covariance(self, hypothesis): # Compute posterior covariance matrix I_KH = id_matrix - kalman_gain @ meas_matrix - post_cov = I_KH @ prior_covar @ I_KH.T \ - + kalman_gain @ meas_covar @ kalman_gain.T + post_cov = ( + I_KH @ prior_covar @ I_KH.T + kalman_gain @ meas_covar @ kalman_gain.T + ) else: - kalman_gain = hypothesis.measurement_prediction.cross_covar @ \ - np.linalg.inv(hypothesis.measurement_prediction.covar) + kalman_gain = hypothesis.measurement_prediction.cross_covar @ np.linalg.inv( + hypothesis.measurement_prediction.covar + ) - post_cov = hypothesis.prediction.covar - kalman_gain @ \ - hypothesis.measurement_prediction.covar @ kalman_gain.T + post_cov = ( + hypothesis.prediction.covar + - kalman_gain @ hypothesis.measurement_prediction.covar @ kalman_gain.T + ) return post_cov.view(CovarianceMatrix), kalman_gain @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): + def predict_measurement( + self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs + ): r"""Predict the measurement implied by the predicted state mean Parameters @@ -257,17 +273,21 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme pred_meas = measurement_model.function(predicted_state, **kwargs) - hh = self._measurement_matrix(predicted_state=predicted_state, - measurement_model=measurement_model, - **kwargs) + hh = self._measurement_matrix( + predicted_state=predicted_state, + measurement_model=measurement_model, + **kwargs + ) # The measurement cross covariance and innovation covariance meas_cross_cov = self._measurement_cross_covariance(predicted_state, hh) innov_cov = self._innovation_covariance( - meas_cross_cov, hh, measurement_model, measurement_noise, **kwargs) + meas_cross_cov, hh, measurement_model, measurement_noise, **kwargs + ) return MeasurementPrediction.from_state( - predicted_state, pred_meas, innov_cov, cross_covar=meas_cross_cov) + predicted_state, pred_meas, innov_cov, cross_covar=meas_cross_cov + ) def update(self, hypothesis, **kwargs): r"""The Kalman update method. Given a hypothesised association between @@ -300,29 +320,34 @@ def update(self, hypothesis, **kwargs): # If not, use the one native to the updater (which might still be # none) measurement_model = hypothesis.measurement.measurement_model - measurement_model = self._check_measurement_model( - measurement_model) + measurement_model = self._check_measurement_model(measurement_model) # Attach the measurement prediction to the hypothesis hypothesis.measurement_prediction = self.predict_measurement( - predicted_state, measurement_model=measurement_model, **kwargs) + predicted_state, measurement_model=measurement_model, **kwargs + ) # Kalman gain and posterior covariance posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis) # Posterior mean - posterior_mean = self._posterior_mean(predicted_state, kalman_gain, - hypothesis.measurement, - hypothesis.measurement_prediction) + posterior_mean = self._posterior_mean( + predicted_state, + kalman_gain, + hypothesis.measurement, + hypothesis.measurement_prediction, + ) if self.force_symmetric_covariance: - posterior_covariance = \ - (posterior_covariance + posterior_covariance.T)/2 + posterior_covariance = (posterior_covariance + posterior_covariance.T) / 2 return Update.from_state( hypothesis.prediction, - posterior_mean, posterior_covariance, - timestamp=hypothesis.measurement.timestamp, hypothesis=hypothesis) + posterior_mean, + posterior_covariance, + timestamp=hypothesis.measurement.timestamp, + hypothesis=hypothesis, + ) class ExtendedKalmanUpdater(KalmanUpdater): @@ -334,18 +359,25 @@ class ExtendedKalmanUpdater(KalmanUpdater): via the matrix :math:`H` accessible via :meth:`~.NonLinearModel.jacobian`. """ + # TODO: Enforce the fact that this version of MeasurementModel must be # TODO: capable of executing :attr:`jacobian()` measurement_model: MeasurementModel = Property( default=None, doc="A measurement model. This need not be defined if a measurement " - "model is provided in the measurement. If no model specified on " - "construction, or in the measurement, then error will be thrown. " - "Must be linear or capable or implement the " - ":meth:`~.NonLinearModel.jacobian`.") - - def _measurement_matrix(self, predicted_state, measurement_model=None, - linearisation_point=None, **kwargs): + "model is provided in the measurement. If no model specified on " + "construction, or in the measurement, then error will be thrown. " + "Must be linear or capable or implement the " + ":meth:`~.NonLinearModel.jacobian`.", + ) + + def _measurement_matrix( + self, + predicted_state, + measurement_model=None, + linearisation_point=None, + **kwargs + ): r"""Return the (via :meth:`NonLinearModel.jacobian`) measurement matrix Parameters @@ -385,29 +417,33 @@ class UnscentedKalmanUpdater(KalmanUpdater): measurement. This is then updated via the standard Kalman update equations. """ + # Can be non-linear and non-differentiable measurement_model: MeasurementModel = Property( default=None, doc="The measurement model to be used. This need not be defined if a " - "measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.") + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.", + ) alpha: float = Property( - default=0.5, - doc="Primary sigma point spread scaling parameter. Default is 0.5.") + default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5." + ) beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " - "true distribution is Gaussian, the value of 2 is optimal. " - "Default is 2") + "true distribution is Gaussian, the value of 2 is optimal. " + "Default is 2", + ) kappa: float = Property( default=None, - doc="Secondary spread scaling parameter. Default is calculated as " - "3-Ns") + doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns", + ) @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): + def predict_measurement( + self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs + ): """Unscented Kalman Filter measurement prediction step. Uses the unscented transform to estimate a Gauss-distributed predicted measurement. @@ -434,17 +470,22 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme measurement_model = self._check_measurement_model(measurement_model) - sigma_points, mean_weights, covar_weights = \ - gauss2sigma(predicted_state, - self.alpha, self.beta, self.kappa) + sigma_points, mean_weights, covar_weights = gauss2sigma( + predicted_state, self.alpha, self.beta, self.kappa + ) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None - meas_pred_mean, meas_pred_covar, cross_covar, *_ = \ - unscented_transform(sigma_points, mean_weights, covar_weights, - measurement_model.function, covar_noise=covar_noise) + meas_pred_mean, meas_pred_covar, cross_covar, *_ = unscented_transform( + sigma_points, + mean_weights, + covar_weights, + measurement_model.function, + covar_noise=covar_noise, + ) return MeasurementPrediction.from_state( - predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar) + predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar + ) class SqrtKalmanUpdater(ExtendedKalmanUpdater): @@ -468,10 +509,12 @@ class SqrtKalmanUpdater(ExtendedKalmanUpdater): Journal, 6:6, 1165-1166 """ + qr_method: bool = Property( default=False, doc="A switch to do the update via a QR decomposition, rather than using the (vector form " - "of) the Potter method.") + "of) the Potter method.", + ) def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ @@ -495,7 +538,9 @@ def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ return predicted_state.sqrt_covar.T @ measurement_matrix.T - def _innovation_covariance(self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs): + def _innovation_covariance( + self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs + ): """Compute the innovation covariance Parameters @@ -559,8 +604,9 @@ def _posterior_covariance(self, hypothesis): """ # Do we already have a measurement model? - measurement_model = \ - self._check_measurement_model(hypothesis.measurement.measurement_model) + measurement_model = self._check_measurement_model( + hypothesis.measurement.measurement_model + ) # Square root of the noise covariance, account for the fact that it may be supplied in one # of two ways try: @@ -576,28 +622,33 @@ def _posterior_covariance(self, hypothesis): # Set up and execute the QR decomposition measdim = measurement_model.ndim_meas zeros = np.zeros((measurement_model.ndim_state, measdim)) - biga = np.block([[sqrt_noise_cov, bigh@sqrt_prior_cov], [zeros, sqrt_prior_cov]]) + biga = np.block( + [[sqrt_noise_cov, bigh @ sqrt_prior_cov], [zeros, sqrt_prior_cov]] + ) _, upper = np.linalg.qr(biga.T) # Extract meaningful quantities atheta = upper.T sqrt_innov_cov = atheta[:measdim, :measdim] - kalman_gain = atheta[measdim:, :measdim]@(np.linalg.inv(sqrt_innov_cov)) + kalman_gain = atheta[measdim:, :measdim] @ (np.linalg.inv(sqrt_innov_cov)) post_cov = atheta[measdim:, measdim:] else: # Kalman gain - kalman_gain = \ - hypothesis.prediction.sqrt_covar @ \ - hypothesis.measurement_prediction.cross_covar @ \ - np.linalg.inv(hypothesis.measurement_prediction.covar) + kalman_gain = ( + hypothesis.prediction.sqrt_covar + @ hypothesis.measurement_prediction.cross_covar + @ np.linalg.inv(hypothesis.measurement_prediction.covar) + ) # Square root of the innovation covariance sqrt_innov_cov = la.sqrtm(hypothesis.measurement_prediction.covar) # Posterior covariance - post_cov = hypothesis.prediction.sqrt_covar @ \ - (np.identity(hypothesis.prediction.ndim) - - hypothesis.measurement_prediction.cross_covar @ np.linalg.inv(sqrt_innov_cov.T) @ - np.linalg.inv(sqrt_innov_cov + sqrt_noise_cov) @ - hypothesis.measurement_prediction.cross_covar.T) + post_cov = hypothesis.prediction.sqrt_covar @ ( + np.identity(hypothesis.prediction.ndim) + - hypothesis.measurement_prediction.cross_covar + @ np.linalg.inv(sqrt_innov_cov.T) + @ np.linalg.inv(sqrt_innov_cov + sqrt_noise_cov) + @ hypothesis.measurement_prediction.cross_covar.T + ) return post_cov, kalman_gain @@ -635,15 +686,18 @@ class IteratedKalmanUpdater(ExtendedKalmanUpdater): tolerance: float = Property( default=1e-6, - doc="The value of the difference in the measure used as a stopping criterion.") + doc="The value of the difference in the measure used as a stopping criterion.", + ) measure: Measure = Property( default=Euclidean(), doc="The measure to use to test the iteration stopping criterion. Defaults to the " - "Euclidean distance between current and prior posterior state estimate.") + "Euclidean distance between current and prior posterior state estimate.", + ) max_iterations: int = Property( default=1000, doc="Number of iterations before while loop is exited and a non-convergence warning is " - "returned") + "returned", + ) def update(self, hypothesis, **kwargs): r"""The iterated Kalman update method. Given a hypothesised association between a predicted @@ -668,7 +722,9 @@ def update(self, hypothesis, **kwargs): """ # Get the measurement model - measurement_model = self._check_measurement_model(hypothesis.measurement.measurement_model) + measurement_model = self._check_measurement_model( + hypothesis.measurement.measurement_model + ) # The first iteration is just the application of the EKF post_state = super().update(hypothesis, **kwargs) @@ -685,16 +741,20 @@ def update(self, hypothesis, **kwargs): # These lines effectively bypass the predict_measurement function in update() # by attaching new linearised quantities to the measurement_prediction. Those # would otherwise be calculated (from the original prediction) by the update() method. - hh = self._measurement_matrix(post_state, measurement_model=measurement_model) + hh = self._measurement_matrix( + post_state, measurement_model=measurement_model + ) - post_state.hypothesis.measurement_prediction.state_vector = \ - measurement_model.function(post_state, noise=None) + \ - hh@(hypothesis.prediction.state_vector - post_state.state_vector) + post_state.hypothesis.measurement_prediction.state_vector = ( + measurement_model.function(post_state, noise=None) + + hh @ (hypothesis.prediction.state_vector - post_state.state_vector) + ) cross_cov = self._measurement_cross_covariance(hypothesis.prediction, hh) post_state.hypothesis.measurement_prediction.cross_covar = cross_cov - post_state.hypothesis.measurement_prediction.covar = \ + post_state.hypothesis.measurement_prediction.covar = ( self._innovation_covariance(cross_cov, hh, measurement_model, True) + ) prev_state = post_state post_state = super().update(post_state.hypothesis, **kwargs) @@ -767,18 +827,23 @@ class SchmidtKalmanUpdater(ExtendedKalmanUpdater): 10.1007/s40295-015-0068-7. """ - consider: np.ndarray = Property(default=None, - doc="The boolean vector of 'consider' parameters. True " - "indicates considered, False are state parameters to be " - "estimated. If undefined these default to all False, i.e." - "the standard Kalman filter.") + + consider: np.ndarray = Property( + default=None, + doc="The boolean vector of 'consider' parameters. True " + "indicates considered, False are state parameters to be " + "estimated. If undefined these default to all False, i.e." + "the standard Kalman filter.", + ) def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) if self.consider is None: self.consider = np.zeros(self.measurement_model.ndim_state, dtype=bool) - def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement_prediction): + def _posterior_mean( + self, predicted_state, kalman_gain, measurement, measurement_prediction + ): """Compute the posterior mean, :math:`s_{k|k} = s_{k|k-1} + K_s (z - H_s s_{k|k-1} - H_p p_{k|k-1})`, :math:`p_{k|k} = p_{k|k-1}. @@ -799,8 +864,9 @@ def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement The posterior mean estimate """ post_mean = predicted_state.state_vector.copy() - post_mean[np.ix_(~self.consider)] += \ - kalman_gain @ (measurement.state_vector - measurement_prediction.state_vector) + post_mean[np.ix_(~self.consider)] += kalman_gain @ ( + measurement.state_vector - measurement_prediction.state_vector + ) return post_mean.view(StateVector) def _posterior_covariance(self, hypothesis): @@ -825,21 +891,25 @@ def _posterior_covariance(self, hypothesis): """ # Intermediate matrices P_p and H. - pp = hypothesis.prediction.covar[np.tile(self.consider, (len(self.consider), 1))] + pp = hypothesis.prediction.covar[ + np.tile(self.consider, (len(self.consider), 1)) + ] pp = pp.reshape((len(self.consider), np.sum(self.consider))) hh = self._measurement_matrix(predicted_state=hypothesis.prediction) # First get the Kalman gain mcc = hypothesis.measurement_prediction.cross_covar - kalman_gain = mcc[np.ix_(~self.consider)] @ \ - np.linalg.inv(hypothesis.measurement_prediction.covar) + kalman_gain = mcc[np.ix_(~self.consider)] @ np.linalg.inv( + hypothesis.measurement_prediction.covar + ) # Then assemble the quadrants of the posterior covariance (easier to think of them as # quadrants even though they're actually submatrices who may appear in somewhat different # places.) post_cov = hypothesis.prediction.covar.copy() - post_cov[np.ix_(~self.consider, ~self.consider)] -= \ + post_cov[np.ix_(~self.consider, ~self.consider)] -= ( kalman_gain @ hypothesis.measurement_prediction.covar @ kalman_gain.T + ) khp = kalman_gain @ hh @ pp post_cov[np.ix_(~self.consider, self.consider)] -= khp post_cov[np.ix_(self.consider, ~self.consider)] -= khp.T @@ -856,20 +926,24 @@ class CubatureKalmanUpdater(KalmanUpdater): update equations. """ + measurement_model: MeasurementModel = Property( default=None, doc="The measurement model to be used. This need not be defined if a " - "measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.") + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.", + ) alpha: float = Property( default=1.0, doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.") + "vice versa.", + ) @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): + def predict_measurement( + self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs + ): """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to estimate a Gauss-distributed predicted measurement. @@ -896,13 +970,16 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme measurement_model = self._check_measurement_model(measurement_model) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None - meas_pred_mean, meas_pred_covar, cross_covar, _ = \ - cubature_transform(predicted_state, - measurement_model.function, - covar_noise=covar_noise, alpha=self.alpha) + meas_pred_mean, meas_pred_covar, cross_covar, _ = cubature_transform( + predicted_state, + measurement_model.function, + covar_noise=covar_noise, + alpha=self.alpha, + ) return MeasurementPrediction.from_state( - predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar) + predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar + ) class StochasticIntegrationUpdater(KalmanUpdater): @@ -913,29 +990,29 @@ class StochasticIntegrationUpdater(KalmanUpdater): the stochastic integration approximation. """ + # Can be non-linear and non-differentiable measurement_model: MeasurementModel = Property( default=None, doc="The measurement model to be used. This need not be defined if a " - "measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.") - Nmax: float = Property( - default=10, - doc="maximal number of iterations of SIR") + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.", + ) + Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") Nmin: float = Property( default=5, - doc="minimal number of iterations of stochastic integration rule (SIR)") - Eps: float = Property( - default=5e-4, - doc="allowed threshold for integration error") + doc="minimal number of iterations of stochastic integration rule (SIR)", + ) + Eps: float = Property(default=5e-4, doc="allowed threshold for integration error") SIorder: float = Property( - default=3, - doc="order of SIR (orders 1, 3, 5 are currently supported)") + default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" + ) @lru_cache() - def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, - **kwargs): + def predict_measurement( + self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs + ): """SIF. Parameters @@ -964,25 +1041,27 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme epMean = predicted_state.mean Iz = np.zeros((nz, 1)) Vz = np.zeros((nz, 1)) - IPz = np.zeros((nz)) + IPz = np.zeros((nz, nz)) VPz = np.zeros((nz)) IPxz = np.zeros((nx, nz)) VPxz = np.zeros((nx, nz)) N = 0 # SIR recursion for measurement predictive moments computation # until either number of iterations is reached or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, - np.any([(np.linalg.norm(Vz) > - self.Eps)])]): + while N < self.Nmin or np.all( + [N < self.Nmax, np.any([(np.linalg.norm(Vz) > self.Eps)])] + ): N = N + 1 # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, - self.SIorder) + [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + \ - np.matlib.repmat(epMean, 1, np.size(SCRSigmaPoints, 1)) + # Use np.tile to replicate the mean vector + epMeanTiled = np.tile(epMean, (1, np.size(SCRSigmaPoints, 1))) + # Matrix multiplication and addition + xpoints = Sp @ SCRSigmaPoints + epMeanTiled + # -- points transformation via measurement equation (deterministic part) sigma_points_states = [] for xpoint in xpoints.T: @@ -990,41 +1069,47 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) - hpoints = StateVectors([ - measurement_model.function(sigma_points_state) - for sigma_points_state in sigma_points_states]) + hpoints = StateVectors( + [ + measurement_model.function(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) # -- stochastic integration rule for predictive measurement mean and covariance # matrix and predictive state and measurement covariance matrix - # SumRz = hpoints@w.reshape(np.size(w),1); - SumRz = np.average(hpoints, axis=1, weights=w) # --- update mean Iz Dz = (SumRz - Iz) / N Iz = Iz + Dz - Vz = (N - 2) * Vz / N + Dz ** 2 + Vz = (N - 2) * Vz / N + Dz**2 # - measurement predictive moments zp = Iz N = 0 - while N < self.Nmin or (N < self.Nmax and (np.linalg.norm(VPz) > self.Eps - or np.linalg.norm(VPxz) > self.Eps)): + while N < self.Nmin or ( + N < self.Nmax + and (np.linalg.norm(VPz) > self.Eps or np.linalg.norm(VPxz) > self.Eps) + ): N += 1 # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) # -- points transformation for given filtering mean and covariance # matrix - xpoints = Sp@SCRSigmaPoints + epMean + xpoints = Sp @ SCRSigmaPoints + epMean # Points transformation via measurement equation (deterministic part) sigma_points_states = [] for xpoint in xpoints.T: state_copy = copy.copy(predicted_state) state_copy.state_vector = StateVector(xpoint) sigma_points_states.append(state_copy) - hpoints = StateVectors([ - measurement_model.function(sigma_points_state) - for sigma_points_state in sigma_points_states]) + hpoints = StateVectors( + [ + measurement_model.function(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) # Stochastic integration rule for predictive measurement mean and covariance # Matrix and predictive state and measurement covariance matrix @@ -1035,22 +1120,25 @@ def predict_measurement(self, predicted_state, measurement_model=None, measureme # Update covariance matrix IPz DPz = (SumRPz - IPz) / N IPz += DPz.reshape(np.shape(IPz)) - VPz = (N - 2)*VPz/N + DPz**2 + VPz = (N - 2) * VPz / N + DPz**2 # Update cross-covariance matrix IPxz DPxz = (SumRPxz - IPxz) / N IPxz += DPxz - VPxz = (N - 2)*VPxz/N + DPxz**2 + VPxz = (N - 2) * VPxz / N + DPxz**2 Pzp = IPz if measurement_noise: - Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) + Pzp = Pzp + measurement_model.covar() + np.diag(Vz.ravel()) else: - Pzp = Pzp + np.diag(Vz.ravel()) - Pzp = Pzp.astype(np.float64) + Pzp = Pzp + np.diag(Vz.ravel()) + Pzp = Pzp.astype(np.float64) Pxzp = IPxz cross_covar = Pxzp.view(CovarianceMatrix) return MeasurementPrediction.from_state( - predicted_state, zp.view(StateVector), Pzp.view(CovarianceMatrix), - cross_covar=cross_covar) + predicted_state, + zp.view(StateVector), + Pzp.view(CovarianceMatrix), + cross_covar=cross_covar, + ) From 860d0a598a367dfafdabcb1a760a22e0f2186e05 Mon Sep 17 00:00:00 2001 From: pesslovany <43780444+pesslovany@users.noreply.github.com> Date: Wed, 7 Aug 2024 09:41:33 +0200 Subject: [PATCH 19/51] Refactoring based on rewievers comments --- stonesoup/functions/__init__.py | 51 +++++++++++++++++++ stonesoup/predictor/kalman.py | 89 ++++++++------------------------- stonesoup/smoother/kalman.py | 33 +++--------- stonesoup/types/state.py | 3 -- stonesoup/updater/kalman.py | 69 ++++++------------------- 5 files changed, 95 insertions(+), 150 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index c0ccf654d..40b421487 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1086,3 +1086,54 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. cross_covar = cross_covar.view(CovarianceMatrix) return mean, covar, cross_covar, cubature_points_t + + +def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): + r""" Calculates cubature points for stochastic integration filter and + puts them through given function (measurement/dynamics) + + Parameters + ========== + nx : integer + dimension + order : integer + order of SIF rule + sqrtCov : array of float 64 [nx x nx] + square root of covariance + mean : array of int 64 [nx x 1] + mean + transFunct : function + function to transfer state vectors + state : state + whole state + + Returns + ======= + points : numpy.ndarray nx x number of points (based on order and dim) + cubature points + w : numpy.ndarray number of points x + weights + trsfPoints : numpy.ndarray nx x number of points (based on order and dim) + cubature transformed points + """ + + # -- cubature points and weights computation (for standard normal PDF) + SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, order) + + # -- points transformation for given filtering mean and covariance matrix + points = sqrtCov @ SCRSigmaPoints + mean + + # -- points transformation through the function + sigma_points_states = [] + for point in points.T: + state_copy = copy.copy(state) + state_copy.state_vector = StateVector(point) + sigma_points_states.append(state_copy) + trsfPoints = StateVectors( + [ + transFunct(sigma_points_state) + for sigma_points_state in sigma_points_states + ] + ) + + return points, w, trsfPoints \ No newline at end of file diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 022916be7..7fb988b56 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -1,4 +1,3 @@ -import copy from functools import partial import numpy as np @@ -6,13 +5,13 @@ from ..base import Property from ..functions import (cubature_transform, gauss2sigma, - stochasticCubatureRulePoints, unscented_transform) + unscented_transform, cubPointsAndTransfer) from ..models.base import LinearModel from ..models.control import ControlModel from ..models.control.linear import LinearControlModel from ..models.transition import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel -from ..types.array import CovarianceMatrix, StateVector, StateVectors +from ..types.array import CovarianceMatrix, StateVector from ..types.prediction import Prediction, SqrtGaussianStatePrediction from ._utils import predict_lru_cache from .base import Predictor @@ -660,7 +659,7 @@ class StochasticIntegrationPredictor(KalmanPredictor): ) Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") SIorder: float = Property( - default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)" + default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) def __init__(self, *args, **kwargs): @@ -736,91 +735,47 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached - while N < self.Nmin or np.all( - [ - N < self.Nmax, - np.any( - [ - (np.linalg.norm(Vx) > self.Eps), - ] - ), - ] - ): - N = N + 1 - # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, self.SIorder) - # -- points transformation for given filtering mean and covariance - # matrix - xpoints = Sp @ SCRSigmaPoints + prior.mean - # -- points transformation via dynamics (deterministic part) - # Put these points into s State object list - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(prior) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - - fpoints = StateVectors( - [ - transition_and_control_function(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + while N < self.Nmin or np.all([N < self.Nmax,np.any([(np.linalg.norm(Vx) > self.Eps),]),]): + N += 1 + + # -- cubature points and weights computation (for standard normal PDF) + # -- points transformation for given filtering mean and covariance matrix + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) + # Stochastic integration rule for predictive state mean and # covariance matrix SumRx = np.average(fpoints, axis=1, weights=w) # --- update mean Ix Dx = (SumRx - Ix) / N - Ix = Ix + Dx - Vx = (N - 2) * Vx / N + Dx**2 + Ix += Dx + Vx = (N - 2) * Vx / N + Dx ** 2 xp = Ix N = 0 # - SIR recursion for state predictive moments computation # -- until max iterations are reached or threshold is reached - while N < self.Nmin or np.all( - [N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])] - ): - N = N + 1 - # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(enx, self.SIorder) - - # Use np.tile to replicate the mean vector - mean_replicated = np.tile(prior.mean, (1, np.size(SCRSigmaPoints, 1))) - # Matrix multiplication and addition - xpoints = Sp @ SCRSigmaPoints + mean_replicated - - # -- points transformation via dynamics (deterministic part) - # Put these points into s State object list - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(prior) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - - fpoints = StateVectors( - [ - transition_and_control_function(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])]): + N += 1 + + # -- cubature points and weights computation (for standard normal PDF) + # -- points transformation for given filtering mean and covariance matrix + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) + # -- stochastic integration rule for predictive state mean and covariance # matrix - fpoints_diff = fpoints - xp SumRPx = fpoints_diff @ np.diag(w) @ fpoints_diff.T # --- update covariance matrix IPx DPx = (SumRPx - IPx) / N - IPx = IPx + DPx - VPx = (N - 2) * VPx / N + DPx**2 + IPx += DPx + VPx = (N - 2) * VPx / N + DPx ** 2 Q = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) - Pp = IPx - Pp = Pp + Q + np.diag(Vx.ravel()) + Pp = IPx + Q + np.diag(Vx.ravel()) Pp = (Pp + Pp.T) / 2 Pp = Pp.astype(np.float64) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index bb94dec09..990283197 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -4,12 +4,10 @@ import numpy as np from ..base import Property -from ..functions import (gauss2sigma, stochasticCubatureRulePoints, - unscented_transform) +from ..functions import (gauss2sigma, unscented_transform, cubPointsAndTransfer) from ..models.base import LinearModel from ..models.transition.base import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel -from ..types.array import StateVector, StateVectors from ..types.multihypothesis import MultipleHypothesis from ..types.prediction import GaussianStatePrediction from ..types.update import GaussianStateUpdate @@ -344,7 +342,7 @@ class StochasticIntegrationSmoother(KalmanSmoother): ) Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") SIorder: float = Property( - default=3, doc="order of SIR (orders 1, 3, 5 are currently supported)" + default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) def _smooth_gain(self, state, prediction, time_interval, **kwargs): @@ -386,37 +384,18 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # - SIR recursion for measurement predictive moments computation # -- until either required number of iterations is reached or threshold is reached while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): + N += 1 - + # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) - # -- points transformation for given filtering mean and covariance matrix - # Use np.tile to replicate the mean vector - efMeanTiled = np.tile(efMean, (1, np.size(SCRSigmaPoints, 1))) - # Matrix multiplication and addition - xpoints = Sf @ SCRSigmaPoints + efMeanTiled - - # -- points transformation via measurement equation (deterministic part) - # fpoints = ffunct(xpoints, wMean, Time) - - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(prediction) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - fpoints = StateVectors( - [ - transition_function(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sf, efMean, transition_function, prediction) # Stochastic integration rule for predictive measurement # mean and covariance matrix fpoints_diff = fpoints - predMean xpoints_diff = xpoints - filtMean - SumRPxx = xpoints_diff @ (fpoints_diff * np.matlib.repmat(w, nx, 1)).T + SumRPxx = xpoints_diff @ (fpoints_diff * np.tile(w,(nx,1))).T # --- update cross-covariance matrix IPxx DPxx = (SumRPxx - IPxx) / N diff --git a/stonesoup/types/state.py b/stonesoup/types/state.py index 51af23d5c..9374d1ddb 100644 --- a/stonesoup/types/state.py +++ b/stonesoup/types/state.py @@ -1008,9 +1008,6 @@ def mean(self): @clearable_cached_property('state_vector') def covar(self): """Sample covariance matrix for ensemble""" - print(type(self.state_vector)) - print(self.ndim) - print(self.num_vectors) return np.cov(self.state_vector) @clearable_cached_property('state_vector') diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 13c5b420a..541b70e79 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1,4 +1,3 @@ -import copy import warnings from functools import lru_cache @@ -7,12 +6,12 @@ from ..base import Property from ..functions import (cubature_transform, gauss2sigma, - stochasticCubatureRulePoints, unscented_transform) + unscented_transform, cubPointsAndTransfer) from ..measures import Euclidean, Measure from ..models.base import LinearModel from ..models.measurement import MeasurementModel from ..models.measurement.linear import LinearGaussian -from ..types.array import CovarianceMatrix, StateVector, StateVectors +from ..types.array import CovarianceMatrix, StateVector from ..types.prediction import MeasurementPrediction from ..types.update import Update from .base import Updater @@ -1048,71 +1047,35 @@ def predict_measurement( N = 0 # SIR recursion for measurement predictive moments computation # until either number of iterations is reached or threshold is reached - while N < self.Nmin or np.all( - [N < self.Nmax, np.any([(np.linalg.norm(Vz) > self.Eps)])] - ): - N = N + 1 + while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(Vz) > self.Eps)])]): + N += 1 + # -- cubature points and weights computation (for standard normal PDF) - [SCRSigmaPoints, w] = stochasticCubatureRulePoints(nx, self.SIorder) - - # -- points transformation for given filtering mean and covariance - # matrix - # Use np.tile to replicate the mean vector - epMeanTiled = np.tile(epMean, (1, np.size(SCRSigmaPoints, 1))) - # Matrix multiplication and addition - xpoints = Sp @ SCRSigmaPoints + epMeanTiled - - # -- points transformation via measurement equation (deterministic part) - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(predicted_state) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - - hpoints = StateVectors( - [ - measurement_model.function(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + # -- points transformation for given filtering mean and covariance matrix + xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) + # -- stochastic integration rule for predictive measurement mean and covariance # matrix and predictive state and measurement covariance matrix SumRz = np.average(hpoints, axis=1, weights=w) # --- update mean Iz Dz = (SumRz - Iz) / N - Iz = Iz + Dz - Vz = (N - 2) * Vz / N + Dz**2 + Iz += Dz.astype(np.float64) + Vz = (N - 2) * Vz / N + Dz ** 2 # - measurement predictive moments zp = Iz N = 0 - while N < self.Nmin or ( - N < self.Nmax - and (np.linalg.norm(VPz) > self.Eps or np.linalg.norm(VPxz) > self.Eps) - ): + while N < self.Nmin or (N < self.Nmax and (np.linalg.norm(VPz) > self.Eps or np.linalg.norm(VPxz) > self.Eps)): N += 1 + # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, self.SIorder) - # -- points transformation for given filtering mean and covariance - # matrix - xpoints = Sp @ SCRSigmaPoints + epMean - # Points transformation via measurement equation (deterministic part) - sigma_points_states = [] - for xpoint in xpoints.T: - state_copy = copy.copy(predicted_state) - state_copy.state_vector = StateVector(xpoint) - sigma_points_states.append(state_copy) - hpoints = StateVectors( - [ - measurement_model.function(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + # -- points transformation for given filtering mean and covariance matrix + xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) + # Stochastic integration rule for predictive measurement mean and covariance # Matrix and predictive state and measurement covariance matrix - hpoints_diff = hpoints - zp SumRPz = hpoints_diff @ np.diag(w) @ hpoints_diff.T SumRPxz = (xpoints - xpoints[:, 0:1]) @ np.diag(w) @ hpoints_diff.T @@ -1124,7 +1087,7 @@ def predict_measurement( # Update cross-covariance matrix IPxz DPxz = (SumRPxz - IPxz) / N IPxz += DPxz - VPxz = (N - 2) * VPxz / N + DPxz**2 + VPxz = (N - 2) * VPxz / N + DPxz ** 2 Pzp = IPz if measurement_noise: From a574a32d74c1d89865d34eb3ae1895fcfe02315b Mon Sep 17 00:00:00 2001 From: pesslovany <43780444+pesslovany@users.noreply.github.com> Date: Wed, 7 Aug 2024 09:54:29 +0200 Subject: [PATCH 20/51] Got rid of loops in the stochasticCubatureRulePoints function --- stonesoup/functions/__init__.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 40b421487..adbe0e03d 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -890,18 +890,16 @@ def stochasticCubatureRulePoints(nx, order): # Calculating sigma points Q = ortho_group.rvs(nx) - v = np.zeros((nx, nx + 1)) - for i in range(0, nx): - v[i, i] = np.sqrt((nx + 1) * (nx - i) / nx / (nx - i + 1)) - for j in range(i + 1, nx + 1): - v[i, j] = -np.sqrt((nx + 1) / ((nx - i) * nx * (nx - i + 1))) + v = np.zeros((nx, nx + 1)) + i_vals, j_vals = np.triu_indices(nx + 1, k=1) + v[i_vals, i_vals] = np.sqrt((nx+1) * (nx-i_vals) / (nx * (nx-i_vals+1))) + v[i_vals, j_vals] = -np.sqrt((nx+1) / ((nx-i_vals) * nx * (nx-i_vals+1))) v = Q @ v - y = np.zeros((nx, int(nx * (nx + 1) / 2))) - cnt = -1 - for j in range(0, nx + 1): - for i in range(0, j): - cnt = cnt + 1 - y[:, cnt] = (v[:, j] + v[:, i]) / np.linalg.norm(v[:, j] + v[:, i], 2) + i_vals, j_vals = np.tril_indices(nx + 1, k=-1) + comb_v = v[:, i_vals] + v[:, j_vals] + y = comb_v / np.linalg.norm(comb_v, axis=0) + + SCRSigmaPoints = np.block( [ From e046aea2fb251bfd676caf1421ca2180559c7763 Mon Sep 17 00:00:00 2001 From: John Hiles Date: Wed, 7 Aug 2024 14:48:59 -0400 Subject: [PATCH 21/51] Fixed flake8 violations for SIF updater, smoother, and predictor --- stonesoup/functions/__init__.py | 47 ++++++++++---------------- stonesoup/predictor/kalman.py | 24 ++++++++----- stonesoup/smoother/kalman.py | 9 ++--- stonesoup/updater/kalman.py | 19 ++++++----- stonesoup/updater/tests/test_kalman.py | 2 ++ 5 files changed, 51 insertions(+), 50 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index adbe0e03d..2f876681c 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -173,12 +173,10 @@ def gauss2sigma(state, alpha=1.0, beta=2.0, kappa=None): sigma_points = sigma_points.astype(float) # Can't use in place addition/subtraction as casting issues may arise when mixing float/int - sigma_points[:, 1 : (ndim_state + 1)] = sigma_points[ - :, 1 : (ndim_state + 1) - ] + sqrt_sigma * np.sqrt(c) - sigma_points[:, (ndim_state + 1) :] = sigma_points[ - :, (ndim_state + 1) : - ] - sqrt_sigma * np.sqrt(c) + sigma_points[:, 1:(ndim_state + 1)] = \ + sigma_points[:, 1:(ndim_state + 1)] + sqrt_sigma*np.sqrt(c) + sigma_points[:, (ndim_state + 1):] = \ + sigma_points[:, (ndim_state + 1):] - sqrt_sigma*np.sqrt(c) # Put these sigma points into s State object list sigma_points_states = [] @@ -890,7 +888,7 @@ def stochasticCubatureRulePoints(nx, order): # Calculating sigma points Q = ortho_group.rvs(nx) - v = np.zeros((nx, nx + 1)) + v = np.zeros((nx, nx + 1)) i_vals, j_vals = np.triu_indices(nx + 1, k=1) v[i_vals, i_vals] = np.sqrt((nx+1) * (nx-i_vals) / (nx * (nx-i_vals+1))) v[i_vals, j_vals] = -np.sqrt((nx+1) / ((nx-i_vals) * nx * (nx-i_vals+1))) @@ -899,8 +897,6 @@ def stochasticCubatureRulePoints(nx, order): comb_v = v[:, i_vals] + v[:, j_vals] y = comb_v / np.linalg.norm(comb_v, axis=0) - - SCRSigmaPoints = np.block( [ np.zeros((nx, 1)), @@ -1006,7 +1002,7 @@ def cubature2gauss(cubature_points, covar_noise=None, alpha=1.0): mean = np.average(cubature_points, axis=1) sigma_mult = cubature_points @ cubature_points.T mean_mult = mean @ mean.T - covar = (1 / alpha) * ((1 / m) * sigma_mult - mean_mult) + covar = (1/alpha)*((1/m)*sigma_mult - mean_mult) if covar_noise is not None: covar = covar + covar_noise @@ -1064,32 +1060,25 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. cubature_points = gauss2cubature(state) if points_noise is None: - cubature_points_t = StateVectors( - [fun(State(cub_point)) for cub_point in cubature_points] - ) + cubature_points_t = StateVectors([fun(State(cub_point)) for cub_point in cubature_points]) else: - cubature_points_t = StateVectors( - [ - fun(State(cub_point), points_noise) - for cub_point, point_noise in zip(cubature_points, points_noise) - ] - ) + cubature_points_t = StateVectors([ + fun(State(cub_point), points_noise) + for cub_point, point_noise in zip(cubature_points, points_noise)]) mean, covar = cubature2gauss(cubature_points_t, covar_noise) - cross_covar = (1 / alpha) * ( - (1.0 / (2 * ndim_state)) * cubature_points @ cubature_points_t.T - - np.average(cubature_points, axis=1) @ mean.T - ) + cross_covar = (1/alpha)*((1./(2*ndim_state))*cubature_points@cubature_points_t.T + - np.average(cubature_points, axis=1)@mean.T) cross_covar = cross_covar.view(CovarianceMatrix) return mean, covar, cross_covar, cubature_points_t def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): - r""" Calculates cubature points for stochastic integration filter and + r""" Calculates cubature points for stochastic integration filter and puts them through given function (measurement/dynamics) - + Parameters ========== nx : integer @@ -1109,12 +1098,12 @@ def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): ======= points : numpy.ndarray nx x number of points (based on order and dim) cubature points - w : numpy.ndarray number of points x + w : numpy.ndarray number of points x weights trsfPoints : numpy.ndarray nx x number of points (based on order and dim) cubature transformed points """ - + # -- cubature points and weights computation (for standard normal PDF) SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, order) @@ -1133,5 +1122,5 @@ def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): for sigma_points_state in sigma_points_states ] ) - - return points, w, trsfPoints \ No newline at end of file + + return points, w, trsfPoints diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 7fb988b56..aabe2bb7b 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -712,7 +712,6 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): """ nx = np.size(prior.mean, 0) - enx = nx Sp = np.linalg.cholesky(prior.covar) Ix = np.zeros((nx, 1)) @@ -735,13 +734,17 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax,np.any([(np.linalg.norm(Vx) > self.Eps),]),]): - N += 1 - + while N < self.Nmin or np.all([N < self.Nmax, + np.any([(np.linalg.norm(Vx) > self.Eps),]),]): + N += 1 + # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) - + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + prior.mean, + transition_and_control_function, + prior) + # Stochastic integration rule for predictive state mean and # covariance matrix SumRx = np.average(fpoints, axis=1, weights=w) @@ -758,11 +761,14 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # -- until max iterations are reached or threshold is reached while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])]): N += 1 - + # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) - + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + prior.mean, + transition_and_control_function, + prior) + # -- stochastic integration rule for predictive state mean and covariance # matrix fpoints_diff = fpoints - xp diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 990283197..322e9a5f1 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -384,18 +384,19 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # - SIR recursion for measurement predictive moments computation # -- until either required number of iterations is reached or threshold is reached while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): - N += 1 - # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sf, efMean, transition_function, prediction) + xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sf, + efMean, + transition_function, + prediction) # Stochastic integration rule for predictive measurement # mean and covariance matrix fpoints_diff = fpoints - predMean xpoints_diff = xpoints - filtMean - SumRPxx = xpoints_diff @ (fpoints_diff * np.tile(w,(nx,1))).T + SumRPxx = xpoints_diff @ (fpoints_diff * np.tile(w, (nx, 1))).T # --- update cross-covariance matrix IPxx DPxx = (SumRPxx - IPxx) / N diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 541b70e79..6166abd82 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1049,15 +1049,15 @@ def predict_measurement( # until either number of iterations is reached or threshold is reached while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(Vz) > self.Eps)])]): N += 1 - # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) - + xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + epMean, + measurement_model.function, + predicted_state) # -- stochastic integration rule for predictive measurement mean and covariance # matrix and predictive state and measurement covariance matrix SumRz = np.average(hpoints, axis=1, weights=w) - # --- update mean Iz Dz = (SumRz - Iz) / N Iz += Dz.astype(np.float64) @@ -1067,13 +1067,16 @@ def predict_measurement( zp = Iz N = 0 - while N < self.Nmin or (N < self.Nmax and (np.linalg.norm(VPz) > self.Eps or np.linalg.norm(VPxz) > self.Eps)): + while N < self.Nmin or (N < self.Nmax and + (np.linalg.norm(VPz) > self.Eps + or np.linalg.norm(VPxz) > self.Eps)): N += 1 - # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) - + xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + epMean, + measurement_model.function, + predicted_state) # Stochastic integration rule for predictive measurement mean and covariance # Matrix and predictive state and measurement covariance matrix hpoints_diff = hpoints - zp diff --git a/stonesoup/updater/tests/test_kalman.py b/stonesoup/updater/tests/test_kalman.py index 3b2c8870e..19c45d023 100644 --- a/stonesoup/updater/tests/test_kalman.py +++ b/stonesoup/updater/tests/test_kalman.py @@ -1,3 +1,5 @@ +"""Test for updater.kalman module""" + import pytest import numpy as np From 1780001244c54f36349db70b38bdc0de9ed018fd Mon Sep 17 00:00:00 2001 From: John Hiles Date: Thu, 8 Aug 2024 14:15:02 -0400 Subject: [PATCH 22/51] Moved SIF files to bottom of respective files. --- stonesoup/functions/__init__.py | 289 ++++++++++++++--------------- stonesoup/predictor/kalman.py | 231 ++++++++--------------- stonesoup/updater/kalman.py | 317 ++++++++++++-------------------- 3 files changed, 332 insertions(+), 505 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 2f876681c..780c6a181 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1,13 +1,12 @@ """Mathematical functions used within Stone Soup""" - import copy import warnings import numpy as np from scipy.stats import ortho_group -from ..types.array import CovarianceMatrix, StateVector, StateVectors from ..types.numeric import Probability +from ..types.array import StateVector, StateVectors, CovarianceMatrix from ..types.state import State @@ -30,7 +29,9 @@ def tria(matrix): _, upper_triangular = np.linalg.qr(matrix.T) lower_triangular = upper_triangular.T - index = [col for col, val in enumerate(np.diag(lower_triangular)) if val < 0] + index = [col + for col, val in enumerate(np.diag(lower_triangular)) + if val < 0] lower_triangular[:, index] *= -1 @@ -61,8 +62,8 @@ def cholesky_eps(A, lower=False): L = np.zeros(A.shape) for i in range(A.shape[0]): for j in range(i): - L[i, j] = (A[i, j] - L[i, :] @ L[j, :].T) / L[j, j] - val = A[i, i] - L[i, :] @ L[i, :].T + L[i, j] = (A[i, j] - L[i, :]@L[j, :].T) / L[j, j] + val = A[i, i] - L[i, :]@L[i, :].T L[i, i] = np.sqrt(val) if val > eps else np.sqrt(eps) if lower: @@ -93,16 +94,13 @@ def jacobian(fun, x, **kwargs): # For numerical reasons the step size needs to large enough. Aim for 1e-8 # relative to spacing between floating point numbers for each dimension - delta = 1e8 * np.spacing(x.state_vector.astype(np.float64).ravel()) + delta = 1e8*np.spacing(x.state_vector.astype(np.float64).ravel()) # But at least 1e-8 # TODO: Is this needed? If not, note special case at zero. delta[delta < 1e-8] = 1e-8 x2 = copy.copy(x) # Create a clone of the input - x2.state_vector = ( - np.tile(x.state_vector, ndim + 1) - + np.eye(ndim, ndim + 1) * delta[:, np.newaxis] - ) + x2.state_vector = np.tile(x.state_vector, ndim+1) + np.eye(ndim, ndim+1)*delta[:, np.newaxis] x2.state_vector = x2.state_vector.view(StateVectors) F = fun(x2, **kwargs) @@ -228,14 +226,8 @@ def sigma2gauss(sigma_points, mean_weights, covar_weights, covar_noise=None): return mean.view(StateVector), covar.view(CovarianceMatrix) -def unscented_transform( - sigma_points_states, - mean_weights, - covar_weights, - fun, - points_noise=None, - covar_noise=None, -): +def unscented_transform(sigma_points_states, mean_weights, covar_weights, + fun, points_noise=None, covar_noise=None): """ Apply the Unscented Transform to a set of sigma points @@ -278,33 +270,24 @@ def unscented_transform( An array containing the transformed sigma point covariance weights """ # Reconstruct the sigma_points matrix - sigma_points = StateVectors( - [sigma_points_state.state_vector for sigma_points_state in sigma_points_states] - ) + sigma_points = StateVectors([ + sigma_points_state.state_vector for sigma_points_state in sigma_points_states]) # Transform points through f if points_noise is None: - sigma_points_t = StateVectors( - [fun(sigma_points_state) for sigma_points_state in sigma_points_states] - ) + sigma_points_t = StateVectors([ + fun(sigma_points_state) for sigma_points_state in sigma_points_states]) else: - sigma_points_t = StateVectors( - [ - fun(sigma_points_state, points_noise) - for sigma_points_state, point_noise in zip( - sigma_points_states, points_noise.T - ) - ] - ) + sigma_points_t = StateVectors([ + fun(sigma_points_state, points_noise) + for sigma_points_state, point_noise in zip(sigma_points_states, points_noise.T)]) # Calculate mean and covariance approximation mean, covar = sigma2gauss(sigma_points_t, mean_weights, covar_weights, covar_noise) # Calculate cross-covariance cross_covar = ( - (sigma_points - sigma_points[:, 0:1]) - @ np.diag(covar_weights) - @ (sigma_points_t - mean).T + (sigma_points-sigma_points[:, 0:1]) @ np.diag(covar_weights) @ (sigma_points_t-mean).T ).view(CovarianceMatrix) return mean, covar, cross_covar, sigma_points_t, mean_weights, covar_weights @@ -468,7 +451,7 @@ def az_el_rg2cart(phi, theta, rho): """ x = rho * np.sin(phi) y = rho * np.sin(theta) - z = rho * np.sqrt(1.0 - np.sin(theta) ** 2 - np.sin(phi) ** 2) + z = rho * np.sqrt(1.0 - np.sin(theta)**2 - np.sin(phi)**2) return x, y, z @@ -502,7 +485,9 @@ def rotx(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[one, zero, zero], [zero, c, -s], [zero, s, c]]) + return np.array([[one, zero, zero], + [zero, c, -s], + [zero, s, c]]) def roty(theta): @@ -536,7 +521,9 @@ def roty(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[c, zero, s], [zero, one, zero], [-s, zero, c]]) + return np.array([[c, zero, s], + [zero, one, zero], + [-s, zero, c]]) def rotz(theta): @@ -570,7 +557,9 @@ def rotz(theta): c, s = np.cos(theta), np.sin(theta) zero = np.zeros_like(theta) one = np.ones_like(theta) - return np.array([[c, -s, zero], [s, c, zero], [zero, zero, one]]) + return np.array([[c, -s, zero], + [s, c, zero], + [zero, zero, one]]) def gm_sample(means, covars, size, weights=None): @@ -608,12 +597,8 @@ def gm_sample(means, covars, size, weights=None): weights = np.array([1 / len(means)] * len(means)) n_samples = np.random.multinomial(size, weights) - samples = np.vstack( - [ - np.random.multivariate_normal(mean.ravel(), covar, sample) - for (mean, covar, sample) in zip(means, covars, n_samples) - ] - ).T + samples = np.vstack([np.random.multivariate_normal(mean.ravel(), covar, sample) + for (mean, covar, sample) in zip(means, covars, n_samples)]).T return StateVectors(samples) @@ -648,10 +633,7 @@ def gm_reduce_single(means, covars, weights): # Calculate covar delta_means = means - mean - covar = ( - np.sum(covars * weights, axis=2, dtype=np.float64) - + weights * delta_means @ delta_means.T - ) + covar = np.sum(covars*weights, axis=2, dtype=np.float64) + weights*delta_means@delta_means.T return mean.view(StateVector), covar.view(CovarianceMatrix) @@ -671,7 +653,7 @@ def mod_bearing(x): Angle in radians in the range math: :math:`-\pi` to :math:`+\pi` """ - x = (x + np.pi) % (2.0 * np.pi) - np.pi + x = (x+np.pi) % (2.0*np.pi)-np.pi return x @@ -690,7 +672,7 @@ def mod_elevation(x): float Angle in radians in the range math: :math:`-\pi/2` to :math:`+\pi/2` """ - x = x % (2 * np.pi) # limit to 2*pi + x = x % (2*np.pi) # limit to 2*pi N = x // (np.pi / 2) # Count # of 90 deg multiples if N == 1: x = np.pi - x @@ -778,9 +760,7 @@ def dotproduct(a, b): """ if np.shape(a) != np.shape(b): - raise ValueError( - "Inputs must be (a collection of) column vectors of the same dimension" - ) + raise ValueError("Inputs must be (a collection of) column vectors of the same dimension") # Decide whether this is a StateVector or a StateVectors if type(a) is StateVector and type(b) is StateVector: @@ -788,9 +768,7 @@ def dotproduct(a, b): elif type(a) is StateVectors and type(b) is StateVectors: return np.atleast_2d(np.asarray(np.sum(a * b, axis=0))) else: - raise ValueError( - "Inputs must be `StateVector` or `StateVectors` and of the same type" - ) + raise ValueError("Inputs must be `StateVector` or `StateVectors` and of the same type") def sde_euler_maruyama_integration(fun, t_values, state_x0): @@ -818,104 +796,10 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0): delta_t = next_t - t delta_w = np.random.normal(scale=np.sqrt(delta_t), size=(state_x.ndim, 1)) a, b = fun(state_x, t) - state_x.state_vector = state_x.state_vector + a * delta_t + b @ delta_w + state_x.state_vector = state_x.state_vector + a*delta_t + b@delta_w return state_x.state_vector -def stochasticCubatureRulePoints(nx, order): - """Stochstic cubature rule points - - computation of cubature points and weights for the stochastic integration - - Parameters - ========== - integer - number of points - - Returns - ======= - numpy.ndarray dim x nx - Matrix of sigma points - """ - - if order == 1: - X = np.random.randn(nx, 1) - SCRSigmaPoints = np.concatenate((X, -X), axis=1) - weights = np.array([0.5, 0.5]) - elif order == 3: - CRSigmaPoints = np.concatenate( - (np.zeros((nx, 1)), np.eye(nx), -np.eye(nx)), axis=1 - ) - rho = np.sqrt(np.random.chisquare(nx + 2)) - Q = ortho_group.rvs(nx) - SCRSigmaPoints = Q * rho @ CRSigmaPoints - weights = np.insert(0.5 * np.ones(2 * nx) / rho**2, 0, (1 - nx / rho**2)) - - elif order == 5: - # generating random values - r = np.sqrt(np.random.chisquare(2 * nx + 7)) - - q = np.random.beta(nx + 2, 3 / 2) - - rho = r * np.sin(np.arcsin(q) / 2) - delta = r * np.cos(np.arcsin(q) / 2) - - # calculating weights - c1up = nx + 2 - delta**2 - c1do = rho**2 * (rho**2 - delta**2) - c2up = nx + 2 - rho**2 - c2do = delta**2 * (delta**2 - rho**2) - cdo = 2 * (nx + 1) ** 2 * (nx + 2) - c3 = (7 - nx) * nx**2 - c4 = 4 * (nx - 1) ** 2 - coef1 = c1up * c3 / cdo / c1do - coef2 = c2up * c3 / cdo / c2do - coef3 = c1up * c4 / cdo / c1do - coef4 = c2up * c4 / cdo / c2do - - pom = np.concatenate( - ( - np.ones(2 * nx + 2) * coef1, - np.ones(2 * nx + 2) * coef2, - np.ones(nx * (nx + 1)) * coef3, - np.ones(nx * (nx + 1)) * coef4, - ), - axis=0, - ) - weights = np.insert( - pom, 0, (1 - nx * (rho**2 + delta**2 - nx - 2) / (rho**2 * delta**2)) - ) - - # Calculating sigma points - Q = ortho_group.rvs(nx) - v = np.zeros((nx, nx + 1)) - i_vals, j_vals = np.triu_indices(nx + 1, k=1) - v[i_vals, i_vals] = np.sqrt((nx+1) * (nx-i_vals) / (nx * (nx-i_vals+1))) - v[i_vals, j_vals] = -np.sqrt((nx+1) / ((nx-i_vals) * nx * (nx-i_vals+1))) - v = Q @ v - i_vals, j_vals = np.tril_indices(nx + 1, k=-1) - comb_v = v[:, i_vals] + v[:, j_vals] - y = comb_v / np.linalg.norm(comb_v, axis=0) - - SCRSigmaPoints = np.block( - [ - np.zeros((nx, 1)), - -rho * v, - rho * v, - -delta * v, - +delta * v, - -rho * y, - rho * y, - -delta * y, - delta * y, - ] - ) - else: - raise ValueError("This order of SIF is not supported") - - return (SCRSigmaPoints, weights) - - def gauss2cubature(state, alpha=1.0): r"""Evaluate the cubature points for an input Gaussian state. This is done under the assumption that the input state is :math:`\mathcal{N}(\mathbf{\mu}, \Sigma)` of dimension :math:`n`. We @@ -950,14 +834,13 @@ def gauss2cubature(state, alpha=1.0): ndim_state = np.shape(state.state_vector)[0] sqrt_covar = np.linalg.cholesky(state.covar) - cuba_points = np.sqrt(alpha * ndim_state) * np.hstack( - (np.identity(ndim_state), -np.identity(ndim_state)) - ) + cuba_points = np.sqrt(alpha*ndim_state) * np.hstack((np.identity(ndim_state), + -np.identity(ndim_state))) if np.issubdtype(cuba_points.dtype, np.integer): cuba_points = cuba_points.astype(float) - cuba_points = sqrt_covar @ cuba_points + state.mean + cuba_points = sqrt_covar@cuba_points + state.mean return StateVectors(cuba_points) @@ -1075,6 +958,100 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. return mean, covar, cross_covar, cubature_points_t +def stochasticCubatureRulePoints(nx, order): + """Stochstic cubature rule points + + computation of cubature points and weights for the stochastic integration + + Parameters + ========== + integer + number of points + + Returns + ======= + numpy.ndarray dim x nx + Matrix of sigma points + """ + + if order == 1: + X = np.random.randn(nx, 1) + SCRSigmaPoints = np.concatenate((X, -X), axis=1) + weights = np.array([0.5, 0.5]) + elif order == 3: + CRSigmaPoints = np.concatenate( + (np.zeros((nx, 1)), np.eye(nx), -np.eye(nx)), axis=1 + ) + rho = np.sqrt(np.random.chisquare(nx + 2)) + Q = ortho_group.rvs(nx) + SCRSigmaPoints = Q * rho @ CRSigmaPoints + weights = np.insert(0.5 * np.ones(2 * nx) / rho**2, 0, (1 - nx / rho**2)) + + elif order == 5: + # generating random values + r = np.sqrt(np.random.chisquare(2 * nx + 7)) + + q = np.random.beta(nx + 2, 3 / 2) + + rho = r * np.sin(np.arcsin(q) / 2) + delta = r * np.cos(np.arcsin(q) / 2) + + # calculating weights + c1up = nx + 2 - delta**2 + c1do = rho**2 * (rho**2 - delta**2) + c2up = nx + 2 - rho**2 + c2do = delta**2 * (delta**2 - rho**2) + cdo = 2 * (nx + 1) ** 2 * (nx + 2) + c3 = (7 - nx) * nx**2 + c4 = 4 * (nx - 1) ** 2 + coef1 = c1up * c3 / cdo / c1do + coef2 = c2up * c3 / cdo / c2do + coef3 = c1up * c4 / cdo / c1do + coef4 = c2up * c4 / cdo / c2do + + pom = np.concatenate( + ( + np.ones(2 * nx + 2) * coef1, + np.ones(2 * nx + 2) * coef2, + np.ones(nx * (nx + 1)) * coef3, + np.ones(nx * (nx + 1)) * coef4, + ), + axis=0, + ) + weights = np.insert( + pom, 0, (1 - nx * (rho**2 + delta**2 - nx - 2) / (rho**2 * delta**2)) + ) + + # Calculating sigma points + Q = ortho_group.rvs(nx) + v = np.zeros((nx, nx + 1)) + i_vals, j_vals = np.triu_indices(nx + 1, k=1) + v[i_vals, i_vals] = np.sqrt((nx+1) * (nx-i_vals) / (nx * (nx-i_vals+1))) + v[i_vals, j_vals] = -np.sqrt((nx+1) / ((nx-i_vals) * nx * (nx-i_vals+1))) + v = Q @ v + i_vals, j_vals = np.tril_indices(nx + 1, k=-1) + comb_v = v[:, i_vals] + v[:, j_vals] + y = comb_v / np.linalg.norm(comb_v, axis=0) + + SCRSigmaPoints = np.block( + [ + np.zeros((nx, 1)), + -rho * v, + rho * v, + -delta * v, + +delta * v, + -rho * y, + rho * y, + -delta * y, + delta * y, + ] + ) + else: + raise ValueError("This order of SIF is not supported") + + return (SCRSigmaPoints, weights) + + def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): r""" Calculates cubature points for stochastic integration filter and puts them through given function (measurement/dynamics) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index aabe2bb7b..356c73e54 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -3,23 +3,23 @@ import numpy as np import scipy.linalg as la +from .base import Predictor +from ._utils import predict_lru_cache from ..base import Property -from ..functions import (cubature_transform, gauss2sigma, - unscented_transform, cubPointsAndTransfer) +from ..types.array import CovarianceMatrix, StateVector +from ..types.prediction import Prediction, SqrtGaussianStatePrediction from ..models.base import LinearModel -from ..models.control import ControlModel -from ..models.control.linear import LinearControlModel from ..models.transition import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel -from ..types.array import CovarianceMatrix, StateVector -from ..types.prediction import Prediction, SqrtGaussianStatePrediction -from ._utils import predict_lru_cache -from .base import Predictor +from ..models.control import ControlModel +from ..models.control.linear import LinearControlModel +from ..functions import (gauss2sigma, unscented_transform, cubature_transform, + cubPointsAndTransfer) class KalmanPredictor(Predictor): r"""A predictor class which forms the basis for the family of Kalman - predictors. This class also servas the (specific) Kalman Filter + predictors. This class also serves as the (specific) Kalman Filter :class:`~.Predictor` class. Here transition and control models must be linear: .. math:: @@ -38,13 +38,11 @@ class KalmanPredictor(Predictor): """ transition_model: LinearGaussianTransitionModel = Property( - doc="The transition model to be used." - ) + doc="The transition model to be used.") control_model: LinearControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.", - ) + "will create a zero-effect linear :class:`~.ControlModel`.") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -53,9 +51,8 @@ def __init__(self, *args, **kwargs): if self.control_model is None: ndims = self.transition_model.ndim_state ndimc = 2 # No control exerted so this doesn't matter. - self.control_model = LinearControlModel( - np.zeros([ndims, ndimc]), control_noise=np.zeros([ndimc, ndimc]) - ) + self.control_model = LinearControlModel(np.zeros([ndims, ndimc]), + control_noise=np.zeros([ndimc, ndimc])) def _transition_matrix(self, **kwargs): """Return the transition matrix @@ -130,9 +127,7 @@ def _predict_over_interval(self, prior, timestamp): return predict_over_interval - def _predicted_covariance( - self, prior, predict_over_interval, control_input=None, **kwargs - ): + def _predicted_covariance(self, prior, predict_over_interval, control_input=None, **kwargs): """Private function to return the predicted covariance. Useful in that it can be overwritten in children. @@ -152,25 +147,17 @@ def _predicted_covariance( """ prior_cov = prior.covar - trans_m = self._transition_matrix( - prior=prior, time_interval=predict_over_interval, **kwargs - ) - trans_cov = self.transition_model.covar( - time_interval=predict_over_interval, **kwargs - ) + trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, + **kwargs) + trans_cov = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) # As this is Kalman-like, the control model must be capable of # returning a control matrix (B) - ctrl_mat = self._control_matrix( - control_input=control_input, time_interval=predict_over_interval, **kwargs - ) + ctrl_mat = self._control_matrix(control_input=control_input, + time_interval=predict_over_interval, **kwargs) ctrl_noi = self.control_model.control_noise - return ( - trans_m @ prior_cov @ trans_m.T - + trans_cov - + ctrl_mat @ ctrl_noi @ ctrl_mat.T - ) + return trans_m @ prior_cov @ trans_m.T + trans_cov + ctrl_mat @ ctrl_noi @ ctrl_mat.T @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -202,25 +189,18 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # Prediction of the mean x_pred = self._transition_function( - prior, time_interval=predict_over_interval, **kwargs - ) + self.control_model.function( - control_input, time_interval=predict_over_interval, **kwargs - ) + prior, time_interval=predict_over_interval, **kwargs) \ + + self.control_model.function(control_input, time_interval=predict_over_interval, + **kwargs) # Prediction of the covariance - p_pred = self._predicted_covariance( - prior, predict_over_interval, control_input=control_input, **kwargs - ) + p_pred = self._predicted_covariance(prior, predict_over_interval, + control_input=control_input, + **kwargs) # And return the state in the correct form - return Prediction.from_state( - prior, - x_pred, - p_pred, - timestamp=timestamp, - transition_model=self.transition_model, - prior=prior, - ) + return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, + transition_model=self.transition_model, prior=prior) class ExtendedKalmanPredictor(KalmanPredictor): @@ -241,8 +221,7 @@ class ExtendedKalmanPredictor(KalmanPredictor): control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.", - ) + "will create a zero-effect linear :class:`~.ControlModel`.") def _transition_matrix(self, prior, linearisation_point=None, **kwargs): r"""Returns the transition matrix, a matrix if the model is linear, or @@ -316,26 +295,23 @@ class UnscentedKalmanPredictor(KalmanPredictor): Gaussian mean and covariance, then putting these through the (in general non-linear) transition function, then reconstructing the Gaussian. """ - transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.", - ) + "will create a zero-effect linear :class:`~.ControlModel`.") alpha: float = Property( - default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5." - ) + default=0.5, + doc="Primary sigma point spread scaling parameter. Default is 0.5.") beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " - "true distribution is Gaussian, the value of 2 is optimal. " - "Default is 2", - ) + "true distribution is Gaussian, the value of 2 is optimal. " + "Default is 2") kappa: float = Property( default=None, - doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns", - ) + doc="Secondary spread scaling parameter. Default is calculated as " + "3-Ns") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -360,9 +336,8 @@ def _transition_and_control_function(self, prior_state, **kwargs): control """ - return self.transition_model.function( - prior_state, **kwargs - ) + self.control_model.function(**kwargs) + return self.transition_model.function(prior_state, **kwargs) + \ + self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -394,47 +369,33 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # TODO: covariances, i.e. sum them before calculating # TODO: the sigma points and then just sticking them into the # TODO: unscented transform, and I haven't checked the statistics. - ctrl_mat = self.control_model.matrix( - time_interval=predict_over_interval, **kwargs - ) + ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = ( - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) + total_noise_covar = \ + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ + ctrl_mat @ ctrl_noi @ ctrl_mat.T - ) # Get the sigma points from the prior mean and covariance. sigma_point_states, mean_weights, covar_weights = gauss2sigma( - prior, self.alpha, self.beta, self.kappa - ) + prior, self.alpha, self.beta, self.kappa) # This ensures that function passed to unscented transform has the # correct time interval transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, - time_interval=predict_over_interval, - ) + time_interval=predict_over_interval) # Put these through the unscented transform, together with the total # covariance to get the parameters of the Gaussian x_pred, p_pred, _, _, _, _ = unscented_transform( - sigma_point_states, - mean_weights, - covar_weights, - transition_and_control_function, - covar_noise=total_noise_covar, + sigma_point_states, mean_weights, covar_weights, + transition_and_control_function, covar_noise=total_noise_covar ) # and return a Gaussian state based on these parameters - return Prediction.from_state( - prior, - x_pred, - p_pred, - timestamp=timestamp, - transition_model=self.transition_model, - prior=prior, - ) + return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, + transition_model=self.transition_model, prior=prior) class SqrtKalmanPredictor(ExtendedKalmanPredictor): @@ -459,19 +420,15 @@ class SqrtKalmanPredictor(ExtendedKalmanPredictor): Springfield, VA. """ - qr_method: bool = Property( default=False, doc="A switch to do the prediction via a QR decomposition, rather than using a Cholesky " - "decomposition.", - ) + "decomposition.") # This predictor returns a square root form of the Gaussian state prediction _prediction_class = SqrtGaussianStatePrediction - def _predicted_covariance( - self, prior, predict_over_interval, control_input=None, **kwargs - ): + def _predicted_covariance(self, prior, predict_over_interval, control_input=None, **kwargs): """Private function to return the predicted covariance. Parameters @@ -491,47 +448,36 @@ def _predicted_covariance( """ sqrt_prior_cov = prior.sqrt_covar - trans_m = self._transition_matrix( - prior=prior, time_interval=predict_over_interval, **kwargs - ) + trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, + **kwargs) try: - sqrt_trans_cov = self.transition_model.sqrt_covar( - time_interval=predict_over_interval, **kwargs - ) + sqrt_trans_cov = self.transition_model.sqrt_covar(time_interval=predict_over_interval, + **kwargs) except AttributeError: - sqrt_trans_cov = la.sqrtm( - self.transition_model.covar( - time_interval=predict_over_interval, **kwargs - ) - ) + sqrt_trans_cov = la.sqrtm(self.transition_model.covar( + time_interval=predict_over_interval, **kwargs)) # As this is Kalman-like, the control model must be capable of returning a control matrix # (B) - ctrl_mat = self._control_matrix( - control_input=control_input, time_interval=predict_over_interval - ) + ctrl_mat = self._control_matrix(control_input=control_input, + time_interval=predict_over_interval) try: - sqrt_ctrl_noi = self.control_model.sqrt_covar( - time_interval=predict_over_interval, **kwargs - ) + sqrt_ctrl_noi = self.control_model.sqrt_covar(time_interval=predict_over_interval, + **kwargs) except AttributeError: - sqrt_ctrl_noi = la.sqrtm( - self.control_model.covar(time_interval=predict_over_interval, **kwargs) - ) + sqrt_ctrl_noi = la.sqrtm(self.control_model.covar(time_interval=predict_over_interval, + **kwargs)) if self.qr_method: # Note that the control matrix aspect of this hasn't been tested - m_sq_trans_cov = np.block( - [[trans_m @ sqrt_prior_cov, sqrt_trans_cov, ctrl_mat @ sqrt_ctrl_noi]] - ) + m_sq_trans_cov = np.block([[trans_m @ sqrt_prior_cov, sqrt_trans_cov, + ctrl_mat@sqrt_ctrl_noi]]) _, pred_sqrt_cov = np.linalg.qr(m_sq_trans_cov.T) return pred_sqrt_cov.T else: - return np.linalg.cholesky( - trans_m @ sqrt_prior_cov @ sqrt_prior_cov.T @ trans_m.T - + sqrt_trans_cov @ sqrt_trans_cov.T - + ctrl_mat @ sqrt_ctrl_noi @ sqrt_ctrl_noi.T @ ctrl_mat.T - ) + return np.linalg.cholesky(trans_m@sqrt_prior_cov@sqrt_prior_cov.T@trans_m.T + + sqrt_trans_cov@sqrt_trans_cov.T + + ctrl_mat@sqrt_ctrl_noi@sqrt_ctrl_noi.T@ctrl_mat.T) class CubatureKalmanPredictor(KalmanPredictor): @@ -543,18 +489,15 @@ class CubatureKalmanPredictor(KalmanPredictor): :meth:`cubature_transform` function. """ - transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " - "will create a zero-effect linear :class:`~.ControlModel`.", - ) + "will create a zero-effect linear :class:`~.ControlModel`.") alpha: float = Property( default=1.0, doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.", - ) + "vice versa.") def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions @@ -574,9 +517,8 @@ def _transition_and_control_function(self, prior_state, **kwargs): control """ - return self.transition_model.function( - prior_state, **kwargs - ) + self.control_model.function(**kwargs) + return self.transition_model.function(prior_state, **kwargs) \ + + self.control_model.function(**kwargs) @predict_lru_cache() def predict(self, prior, timestamp=None, control_input=None, **kwargs): @@ -605,38 +547,24 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # The covariance on the transition model + the control model # TODO: See equivalent note to unscented transform. - ctrl_mat = self.control_model.matrix( - time_interval=predict_over_interval, **kwargs - ) + ctrl_mat = self.control_model.matrix(time_interval=predict_over_interval, **kwargs) ctrl_noi = self.control_model.covar(**kwargs) - total_noise_covar = ( - self.transition_model.covar(time_interval=predict_over_interval, **kwargs) - + ctrl_mat @ ctrl_noi @ ctrl_mat.T - ) + total_noise_covar = \ + self.transition_model.covar(time_interval=predict_over_interval, **kwargs) \ + + ctrl_mat@ctrl_noi@ctrl_mat.T # This ensures that function passed to transform has the correct time interval and control # input transition_and_control_function = partial( self._transition_and_control_function, control_input=control_input, - time_interval=predict_over_interval, - ) + time_interval=predict_over_interval) - x_pred, p_pred, _, _ = cubature_transform( - prior, - transition_and_control_function, - covar_noise=total_noise_covar, - alpha=self.alpha, - ) + x_pred, p_pred, _, _ = cubature_transform(prior, transition_and_control_function, + covar_noise=total_noise_covar, alpha=self.alpha) - return Prediction.from_state( - prior, - x_pred, - p_pred, - timestamp=timestamp, - transition_model=self.transition_model, - prior=prior, - ) + return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, + transition_model=self.transition_model, prior=prior) class StochasticIntegrationPredictor(KalmanPredictor): @@ -761,7 +689,6 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # -- until max iterations are reached or threshold is reached while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])]): N += 1 - # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 6166abd82..8f719beeb 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1,20 +1,20 @@ import warnings -from functools import lru_cache import numpy as np import scipy.linalg as la +from functools import lru_cache from ..base import Property -from ..functions import (cubature_transform, gauss2sigma, - unscented_transform, cubPointsAndTransfer) -from ..measures import Euclidean, Measure -from ..models.base import LinearModel -from ..models.measurement import MeasurementModel -from ..models.measurement.linear import LinearGaussian +from .base import Updater from ..types.array import CovarianceMatrix, StateVector from ..types.prediction import MeasurementPrediction from ..types.update import Update -from .base import Updater +from ..models.base import LinearModel +from ..models.measurement.linear import LinearGaussian +from ..models.measurement import MeasurementModel +from ..functions import (gauss2sigma, unscented_transform, cubature_transform, + cubPointsAndTransfer) +from ..measures import Measure, Euclidean class KalmanUpdater(Updater): @@ -64,24 +64,20 @@ class KalmanUpdater(Updater): measurement_model: LinearGaussian = Property( default=None, doc="A linear Gaussian measurement model. This need not be defined if " - "a measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.", - ) + "a measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") force_symmetric_covariance: bool = Property( default=False, doc="A flag to force the output covariance matrix to be symmetric by way of a simple " - "geometric combination of the matrix and transpose. Default is False.", - ) + "geometric combination of the matrix and transpose. Default is False.") use_joseph_cov: bool = Property( default=False, doc="Bool dictating the method of covariance calculation. If use_joseph_cov is True then " - "the Joseph form of the covariance equation is used.", - ) + "the Joseph form of the covariance equation is used.") - def _measurement_matrix( - self, predicted_state=None, measurement_model=None, **kwargs - ): + def _measurement_matrix(self, predicted_state=None, measurement_model=None, + **kwargs): r"""This is straightforward Kalman so just get the Matrix from the measurement model. @@ -101,7 +97,8 @@ def _measurement_matrix( The measurement matrix, :math:`H_k` """ - return self._check_measurement_model(measurement_model).matrix(**kwargs) + return self._check_measurement_model( + measurement_model).matrix(**kwargs) def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ @@ -123,9 +120,7 @@ def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ return predicted_state.covar @ measurement_matrix.T - def _innovation_covariance( - self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs - ): + def _innovation_covariance(self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs): """Compute the innovation covariance Parameters @@ -150,9 +145,7 @@ def _innovation_covariance( innov_covar += meas_mod.covar(**kwargs) return innov_covar - def _posterior_mean( - self, predicted_state, kalman_gain, measurement, measurement_prediction - ): + def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement_prediction): r"""Compute the posterior mean, :math:`\mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + K_k \mathbf{y}_k`, where the innovation :math:`\mathbf{y}_k = \mathbf{z}_k - h(\mathbf{x}_{k|k-1}). @@ -173,9 +166,8 @@ def _posterior_mean( : :class:`StateVector` The posterior mean estimate """ - post_mean = predicted_state.state_vector + kalman_gain @ ( - measurement.state_vector - measurement_prediction.state_vector - ) + post_mean = predicted_state.state_vector + \ + kalman_gain @ (measurement.state_vector - measurement_prediction.state_vector) return post_mean.view(StateVector) def _posterior_covariance(self, hypothesis): @@ -203,18 +195,15 @@ def _posterior_covariance(self, hypothesis): id_matrix = np.identity(hypothesis.prediction.ndim) # Calculate Kalman gain - kalman_gain = hypothesis.measurement_prediction.cross_covar @ np.linalg.inv( - hypothesis.measurement_prediction.covar - ) + kalman_gain = hypothesis.measurement_prediction.cross_covar @ \ + np.linalg.inv(hypothesis.measurement_prediction.covar) measurement_model = self._check_measurement_model( - hypothesis.measurement.measurement_model - ) + hypothesis.measurement.measurement_model) # Calculate measurement matrix/jacobian matrix - meas_matrix = self._measurement_matrix( - hypothesis.prediction, measurement_model - ) + meas_matrix = self._measurement_matrix(hypothesis.prediction, + measurement_model) # Calculate Prior covariance prior_covar = hypothesis.prediction.covar @@ -224,26 +213,21 @@ def _posterior_covariance(self, hypothesis): # Compute posterior covariance matrix I_KH = id_matrix - kalman_gain @ meas_matrix - post_cov = ( - I_KH @ prior_covar @ I_KH.T + kalman_gain @ meas_covar @ kalman_gain.T - ) + post_cov = I_KH @ prior_covar @ I_KH.T \ + + kalman_gain @ meas_covar @ kalman_gain.T else: - kalman_gain = hypothesis.measurement_prediction.cross_covar @ np.linalg.inv( - hypothesis.measurement_prediction.covar - ) + kalman_gain = hypothesis.measurement_prediction.cross_covar @ \ + np.linalg.inv(hypothesis.measurement_prediction.covar) - post_cov = ( - hypothesis.prediction.covar - - kalman_gain @ hypothesis.measurement_prediction.covar @ kalman_gain.T - ) + post_cov = hypothesis.prediction.covar - kalman_gain @ \ + hypothesis.measurement_prediction.covar @ kalman_gain.T return post_cov.view(CovarianceMatrix), kalman_gain @lru_cache() - def predict_measurement( - self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs - ): + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, + **kwargs): r"""Predict the measurement implied by the predicted state mean Parameters @@ -272,21 +256,17 @@ def predict_measurement( pred_meas = measurement_model.function(predicted_state, **kwargs) - hh = self._measurement_matrix( - predicted_state=predicted_state, - measurement_model=measurement_model, - **kwargs - ) + hh = self._measurement_matrix(predicted_state=predicted_state, + measurement_model=measurement_model, + **kwargs) # The measurement cross covariance and innovation covariance meas_cross_cov = self._measurement_cross_covariance(predicted_state, hh) innov_cov = self._innovation_covariance( - meas_cross_cov, hh, measurement_model, measurement_noise, **kwargs - ) + meas_cross_cov, hh, measurement_model, measurement_noise, **kwargs) return MeasurementPrediction.from_state( - predicted_state, pred_meas, innov_cov, cross_covar=meas_cross_cov - ) + predicted_state, pred_meas, innov_cov, cross_covar=meas_cross_cov) def update(self, hypothesis, **kwargs): r"""The Kalman update method. Given a hypothesised association between @@ -319,34 +299,29 @@ def update(self, hypothesis, **kwargs): # If not, use the one native to the updater (which might still be # none) measurement_model = hypothesis.measurement.measurement_model - measurement_model = self._check_measurement_model(measurement_model) + measurement_model = self._check_measurement_model( + measurement_model) # Attach the measurement prediction to the hypothesis hypothesis.measurement_prediction = self.predict_measurement( - predicted_state, measurement_model=measurement_model, **kwargs - ) + predicted_state, measurement_model=measurement_model, **kwargs) # Kalman gain and posterior covariance posterior_covariance, kalman_gain = self._posterior_covariance(hypothesis) # Posterior mean - posterior_mean = self._posterior_mean( - predicted_state, - kalman_gain, - hypothesis.measurement, - hypothesis.measurement_prediction, - ) + posterior_mean = self._posterior_mean(predicted_state, kalman_gain, + hypothesis.measurement, + hypothesis.measurement_prediction) if self.force_symmetric_covariance: - posterior_covariance = (posterior_covariance + posterior_covariance.T) / 2 + posterior_covariance = \ + (posterior_covariance + posterior_covariance.T)/2 return Update.from_state( hypothesis.prediction, - posterior_mean, - posterior_covariance, - timestamp=hypothesis.measurement.timestamp, - hypothesis=hypothesis, - ) + posterior_mean, posterior_covariance, + timestamp=hypothesis.measurement.timestamp, hypothesis=hypothesis) class ExtendedKalmanUpdater(KalmanUpdater): @@ -358,25 +333,18 @@ class ExtendedKalmanUpdater(KalmanUpdater): via the matrix :math:`H` accessible via :meth:`~.NonLinearModel.jacobian`. """ - # TODO: Enforce the fact that this version of MeasurementModel must be # TODO: capable of executing :attr:`jacobian()` measurement_model: MeasurementModel = Property( default=None, doc="A measurement model. This need not be defined if a measurement " - "model is provided in the measurement. If no model specified on " - "construction, or in the measurement, then error will be thrown. " - "Must be linear or capable or implement the " - ":meth:`~.NonLinearModel.jacobian`.", - ) + "model is provided in the measurement. If no model specified on " + "construction, or in the measurement, then error will be thrown. " + "Must be linear or capable or implement the " + ":meth:`~.NonLinearModel.jacobian`.") - def _measurement_matrix( - self, - predicted_state, - measurement_model=None, - linearisation_point=None, - **kwargs - ): + def _measurement_matrix(self, predicted_state, measurement_model=None, + linearisation_point=None, **kwargs): r"""Return the (via :meth:`NonLinearModel.jacobian`) measurement matrix Parameters @@ -416,33 +384,29 @@ class UnscentedKalmanUpdater(KalmanUpdater): measurement. This is then updated via the standard Kalman update equations. """ - # Can be non-linear and non-differentiable measurement_model: MeasurementModel = Property( default=None, doc="The measurement model to be used. This need not be defined if a " - "measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.", - ) + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") alpha: float = Property( - default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5." - ) + default=0.5, + doc="Primary sigma point spread scaling parameter. Default is 0.5.") beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " - "true distribution is Gaussian, the value of 2 is optimal. " - "Default is 2", - ) + "true distribution is Gaussian, the value of 2 is optimal. " + "Default is 2") kappa: float = Property( default=None, - doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns", - ) + doc="Secondary spread scaling parameter. Default is calculated as " + "3-Ns") @lru_cache() - def predict_measurement( - self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs - ): + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, + **kwargs): """Unscented Kalman Filter measurement prediction step. Uses the unscented transform to estimate a Gauss-distributed predicted measurement. @@ -469,22 +433,17 @@ def predict_measurement( measurement_model = self._check_measurement_model(measurement_model) - sigma_points, mean_weights, covar_weights = gauss2sigma( - predicted_state, self.alpha, self.beta, self.kappa - ) + sigma_points, mean_weights, covar_weights = \ + gauss2sigma(predicted_state, + self.alpha, self.beta, self.kappa) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None - meas_pred_mean, meas_pred_covar, cross_covar, *_ = unscented_transform( - sigma_points, - mean_weights, - covar_weights, - measurement_model.function, - covar_noise=covar_noise, - ) + meas_pred_mean, meas_pred_covar, cross_covar, *_ = \ + unscented_transform(sigma_points, mean_weights, covar_weights, + measurement_model.function, covar_noise=covar_noise) return MeasurementPrediction.from_state( - predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar - ) + predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar) class SqrtKalmanUpdater(ExtendedKalmanUpdater): @@ -508,12 +467,10 @@ class SqrtKalmanUpdater(ExtendedKalmanUpdater): Journal, 6:6, 1165-1166 """ - qr_method: bool = Property( default=False, doc="A switch to do the update via a QR decomposition, rather than using the (vector form " - "of) the Potter method.", - ) + "of) the Potter method.") def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ @@ -537,9 +494,7 @@ def _measurement_cross_covariance(self, predicted_state, measurement_matrix): """ return predicted_state.sqrt_covar.T @ measurement_matrix.T - def _innovation_covariance( - self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs - ): + def _innovation_covariance(self, m_cross_cov, meas_mat, meas_mod, measurement_noise, **kwargs): """Compute the innovation covariance Parameters @@ -603,9 +558,8 @@ def _posterior_covariance(self, hypothesis): """ # Do we already have a measurement model? - measurement_model = self._check_measurement_model( - hypothesis.measurement.measurement_model - ) + measurement_model = \ + self._check_measurement_model(hypothesis.measurement.measurement_model) # Square root of the noise covariance, account for the fact that it may be supplied in one # of two ways try: @@ -621,33 +575,28 @@ def _posterior_covariance(self, hypothesis): # Set up and execute the QR decomposition measdim = measurement_model.ndim_meas zeros = np.zeros((measurement_model.ndim_state, measdim)) - biga = np.block( - [[sqrt_noise_cov, bigh @ sqrt_prior_cov], [zeros, sqrt_prior_cov]] - ) + biga = np.block([[sqrt_noise_cov, bigh@sqrt_prior_cov], [zeros, sqrt_prior_cov]]) _, upper = np.linalg.qr(biga.T) # Extract meaningful quantities atheta = upper.T sqrt_innov_cov = atheta[:measdim, :measdim] - kalman_gain = atheta[measdim:, :measdim] @ (np.linalg.inv(sqrt_innov_cov)) + kalman_gain = atheta[measdim:, :measdim]@(np.linalg.inv(sqrt_innov_cov)) post_cov = atheta[measdim:, measdim:] else: # Kalman gain - kalman_gain = ( - hypothesis.prediction.sqrt_covar - @ hypothesis.measurement_prediction.cross_covar - @ np.linalg.inv(hypothesis.measurement_prediction.covar) - ) + kalman_gain = \ + hypothesis.prediction.sqrt_covar @ \ + hypothesis.measurement_prediction.cross_covar @ \ + np.linalg.inv(hypothesis.measurement_prediction.covar) # Square root of the innovation covariance sqrt_innov_cov = la.sqrtm(hypothesis.measurement_prediction.covar) # Posterior covariance - post_cov = hypothesis.prediction.sqrt_covar @ ( - np.identity(hypothesis.prediction.ndim) - - hypothesis.measurement_prediction.cross_covar - @ np.linalg.inv(sqrt_innov_cov.T) - @ np.linalg.inv(sqrt_innov_cov + sqrt_noise_cov) - @ hypothesis.measurement_prediction.cross_covar.T - ) + post_cov = hypothesis.prediction.sqrt_covar @ \ + (np.identity(hypothesis.prediction.ndim) - + hypothesis.measurement_prediction.cross_covar @ np.linalg.inv(sqrt_innov_cov.T) @ + np.linalg.inv(sqrt_innov_cov + sqrt_noise_cov) @ + hypothesis.measurement_prediction.cross_covar.T) return post_cov, kalman_gain @@ -685,18 +634,15 @@ class IteratedKalmanUpdater(ExtendedKalmanUpdater): tolerance: float = Property( default=1e-6, - doc="The value of the difference in the measure used as a stopping criterion.", - ) + doc="The value of the difference in the measure used as a stopping criterion.") measure: Measure = Property( default=Euclidean(), doc="The measure to use to test the iteration stopping criterion. Defaults to the " - "Euclidean distance between current and prior posterior state estimate.", - ) + "Euclidean distance between current and prior posterior state estimate.") max_iterations: int = Property( default=1000, doc="Number of iterations before while loop is exited and a non-convergence warning is " - "returned", - ) + "returned") def update(self, hypothesis, **kwargs): r"""The iterated Kalman update method. Given a hypothesised association between a predicted @@ -721,9 +667,7 @@ def update(self, hypothesis, **kwargs): """ # Get the measurement model - measurement_model = self._check_measurement_model( - hypothesis.measurement.measurement_model - ) + measurement_model = self._check_measurement_model(hypothesis.measurement.measurement_model) # The first iteration is just the application of the EKF post_state = super().update(hypothesis, **kwargs) @@ -740,20 +684,16 @@ def update(self, hypothesis, **kwargs): # These lines effectively bypass the predict_measurement function in update() # by attaching new linearised quantities to the measurement_prediction. Those # would otherwise be calculated (from the original prediction) by the update() method. - hh = self._measurement_matrix( - post_state, measurement_model=measurement_model - ) + hh = self._measurement_matrix(post_state, measurement_model=measurement_model) - post_state.hypothesis.measurement_prediction.state_vector = ( - measurement_model.function(post_state, noise=None) - + hh @ (hypothesis.prediction.state_vector - post_state.state_vector) - ) + post_state.hypothesis.measurement_prediction.state_vector = \ + measurement_model.function(post_state, noise=None) + \ + hh@(hypothesis.prediction.state_vector - post_state.state_vector) cross_cov = self._measurement_cross_covariance(hypothesis.prediction, hh) post_state.hypothesis.measurement_prediction.cross_covar = cross_cov - post_state.hypothesis.measurement_prediction.covar = ( + post_state.hypothesis.measurement_prediction.covar = \ self._innovation_covariance(cross_cov, hh, measurement_model, True) - ) prev_state = post_state post_state = super().update(post_state.hypothesis, **kwargs) @@ -826,23 +766,18 @@ class SchmidtKalmanUpdater(ExtendedKalmanUpdater): 10.1007/s40295-015-0068-7. """ - - consider: np.ndarray = Property( - default=None, - doc="The boolean vector of 'consider' parameters. True " - "indicates considered, False are state parameters to be " - "estimated. If undefined these default to all False, i.e." - "the standard Kalman filter.", - ) + consider: np.ndarray = Property(default=None, + doc="The boolean vector of 'consider' parameters. True " + "indicates considered, False are state parameters to be " + "estimated. If undefined these default to all False, i.e." + "the standard Kalman filter.") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) if self.consider is None: self.consider = np.zeros(self.measurement_model.ndim_state, dtype=bool) - def _posterior_mean( - self, predicted_state, kalman_gain, measurement, measurement_prediction - ): + def _posterior_mean(self, predicted_state, kalman_gain, measurement, measurement_prediction): """Compute the posterior mean, :math:`s_{k|k} = s_{k|k-1} + K_s (z - H_s s_{k|k-1} - H_p p_{k|k-1})`, :math:`p_{k|k} = p_{k|k-1}. @@ -863,9 +798,8 @@ def _posterior_mean( The posterior mean estimate """ post_mean = predicted_state.state_vector.copy() - post_mean[np.ix_(~self.consider)] += kalman_gain @ ( - measurement.state_vector - measurement_prediction.state_vector - ) + post_mean[np.ix_(~self.consider)] += \ + kalman_gain @ (measurement.state_vector - measurement_prediction.state_vector) return post_mean.view(StateVector) def _posterior_covariance(self, hypothesis): @@ -890,25 +824,21 @@ def _posterior_covariance(self, hypothesis): """ # Intermediate matrices P_p and H. - pp = hypothesis.prediction.covar[ - np.tile(self.consider, (len(self.consider), 1)) - ] + pp = hypothesis.prediction.covar[np.tile(self.consider, (len(self.consider), 1))] pp = pp.reshape((len(self.consider), np.sum(self.consider))) hh = self._measurement_matrix(predicted_state=hypothesis.prediction) # First get the Kalman gain mcc = hypothesis.measurement_prediction.cross_covar - kalman_gain = mcc[np.ix_(~self.consider)] @ np.linalg.inv( - hypothesis.measurement_prediction.covar - ) + kalman_gain = mcc[np.ix_(~self.consider)] @ \ + np.linalg.inv(hypothesis.measurement_prediction.covar) # Then assemble the quadrants of the posterior covariance (easier to think of them as # quadrants even though they're actually submatrices who may appear in somewhat different # places.) post_cov = hypothesis.prediction.covar.copy() - post_cov[np.ix_(~self.consider, ~self.consider)] -= ( + post_cov[np.ix_(~self.consider, ~self.consider)] -= \ kalman_gain @ hypothesis.measurement_prediction.covar @ kalman_gain.T - ) khp = kalman_gain @ hh @ pp post_cov[np.ix_(~self.consider, self.consider)] -= khp post_cov[np.ix_(self.consider, ~self.consider)] -= khp.T @@ -925,24 +855,20 @@ class CubatureKalmanUpdater(KalmanUpdater): update equations. """ - measurement_model: MeasurementModel = Property( default=None, doc="The measurement model to be used. This need not be defined if a " - "measurement model is provided in the measurement. If no model " - "specified on construction, or in the measurement, then error " - "will be thrown.", - ) + "measurement model is provided in the measurement. If no model " + "specified on construction, or in the measurement, then error " + "will be thrown.") alpha: float = Property( default=1.0, doc="Scaling parameter. Default is 1.0. Lower values select points closer to the mean and " - "vice versa.", - ) + "vice versa.") @lru_cache() - def predict_measurement( - self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs - ): + def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True, + **kwargs): """Cubature Kalman Filter measurement prediction step. Uses the cubature transform to estimate a Gauss-distributed predicted measurement. @@ -969,16 +895,13 @@ def predict_measurement( measurement_model = self._check_measurement_model(measurement_model) covar_noise = measurement_model.covar(**kwargs) if measurement_noise else None - meas_pred_mean, meas_pred_covar, cross_covar, _ = cubature_transform( - predicted_state, - measurement_model.function, - covar_noise=covar_noise, - alpha=self.alpha, - ) + meas_pred_mean, meas_pred_covar, cross_covar, _ = \ + cubature_transform(predicted_state, + measurement_model.function, + covar_noise=covar_noise, alpha=self.alpha) return MeasurementPrediction.from_state( - predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar - ) + predicted_state, meas_pred_mean, meas_pred_covar, cross_covar=cross_covar) class StochasticIntegrationUpdater(KalmanUpdater): From dc04975214fd240403e13dfea7c396632e4ce125 Mon Sep 17 00:00:00 2001 From: John Hiles Date: Thu, 8 Aug 2024 15:16:53 -0400 Subject: [PATCH 23/51] Fixed flake8 style violations in SIF predictor and updater introduced in a merge. --- stonesoup/functions/__init__.py | 82 --------------------------------- stonesoup/predictor/kalman.py | 5 +- stonesoup/updater/kalman.py | 3 +- 3 files changed, 2 insertions(+), 88 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index b278692ae..591adbde2 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -846,88 +846,6 @@ def RandOrthMat(n, tol=1e-6): return (M) -def stochasticCubatureRulePoints(nx, order): - """Stochstic cubature rule points - - computation of cubature points and weights for the stochastic integration - - Parameters - ========== - integer - number of points - - Returns - ======= - numpy.ndarray dim x nx - Matrix of sigma points - """ - - if order == 1: - X = np.random.randn(nx, 1) - SCRSigmaPoints = np.concatenate((X, -X), axis=1) - weights = (np.array([0.5, 0.5])) - if order == 3: - CRSigmaPoints = np.concatenate((np.zeros((nx, 1)), - np.eye(nx), -np.eye(nx)), axis=1) - rho = np.sqrt(np.random.chisquare(nx + 2)) - Q = RandOrthMat(nx) - SCRSigmaPoints = Q * rho @ CRSigmaPoints - weights = np.insert(0.5 * np.ones(2 * nx) / rho ** 2, 0, - (1 - nx / rho ** 2)) - - if order == 5: - # generating random values - r = np.sqrt(np.random.chisquare(2 * nx + 7)) - - q = np.random.beta(nx + 2, 3 / 2) - - rho = r * np.sin(np.arcsin(q) / 2) - delta = r * np.cos(np.arcsin(q) / 2) - - # calculating weights - c1up = nx + 2 - delta ** 2 - c1do = rho ** 2 * (rho ** 2 - delta ** 2) - c2up = nx + 2 - rho ** 2 - c2do = delta ** 2 * (delta ** 2 - rho ** 2) - cdo = 2 * (nx + 1) ** 2 * (nx + 2) - c3 = (7 - nx) * nx ** 2 - c4 = 4 * (nx - 1) ** 2 - coef1 = c1up * c3 / cdo / c1do - coef2 = c2up * c3 / cdo / c2do - coef3 = c1up * c4 / cdo / c1do - coef4 = c2up * c4 / cdo / c2do - - pom = np.concatenate((np.ones(2 * nx + 2) * coef1, - np.ones(2 * nx + 2) * coef2, - np.ones(nx * (nx + 1)) * coef3, - np.ones(nx * (nx + 1)) * coef4), - axis=0) - weights = np.insert(pom, 0, - (1 - nx * (rho ** 2 + delta ** 2 - nx - 2) / - (rho ** 2 * delta ** 2))) - - # Calculating sigma points - Q = RandOrthMat(nx) - v = np.zeros((nx, nx + 1)) - for i in range(0, nx): - v[i, i] = np.sqrt((nx + 1)*(nx - i) / nx / (nx - i + 1)) - for j in range(i + 1, nx + 1): - v[i, j] = -np.sqrt((nx + 1) / ((nx - i) * nx * (nx - i + 1))) - v = Q @ v - y = np.zeros((nx, int(nx*(nx + 1) / 2))) - cnt = -1 - for j in range(0, nx + 1): - for i in range(0, j): - cnt = cnt + 1 - y[:, cnt] = (v[:, j] + v[:, i]) / np.linalg.norm( - v[:, j] + v[:, i], 2) - - SCRSigmaPoints = np.block([np.zeros((nx, 1)), - -rho*v, rho*v, -delta*v, +delta*v, -rho*y, - rho*y, -delta*y, delta*y]) - return (SCRSigmaPoints, weights) - - def gauss2cubature(state, alpha=1.0): r"""Evaluate the cubature points for an input Gaussian state. This is done under the assumption that the input state is :math:`\mathcal{N}(\mathbf{\mu}, \Sigma)` of dimension :math:`n`. We diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index f13c58c68..2763437e5 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -1,15 +1,12 @@ from functools import partial import numpy as np -import numpy.matlib as npm import scipy.linalg as la -import copy -from ..types.array import CovarianceMatrix, StateVector, StateVectors +from ..types.array import CovarianceMatrix, StateVector from .base import Predictor from ._utils import predict_lru_cache from ..base import Property -from ..types.array import CovarianceMatrix, StateVector from ..types.prediction import Prediction, SqrtGaussianStatePrediction from ..models.base import LinearModel from ..models.transition import TransitionModel diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 0edd4682b..8f719beeb 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1,13 +1,12 @@ import warnings -import copy import numpy as np import scipy.linalg as la from functools import lru_cache from ..base import Property from .base import Updater -from ..types.array import CovarianceMatrix, StateVector, StateVectors +from ..types.array import CovarianceMatrix, StateVector from ..types.prediction import MeasurementPrediction from ..types.update import Update from ..models.base import LinearModel From 47ea9446954607dbc1f736741b376f664542b67f Mon Sep 17 00:00:00 2001 From: John Hiles Date: Fri, 9 Aug 2024 12:45:38 -0400 Subject: [PATCH 24/51] Removed unnecisary RandOrthMat function from SIF contribution. --- stonesoup/functions/__init__.py | 46 --------------------------------- 1 file changed, 46 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 591adbde2..780c6a181 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -800,52 +800,6 @@ def sde_euler_maruyama_integration(fun, t_values, state_x0): return state_x.state_vector -def RandOrthMat(n, tol=1e-6): - """Random orthogonal real matrix - - M = RANDORTHMAT(n) - generates a random n x n orthogonal real matrix. - M = RANDORTHMAT(n,tol) - explicitly specifies a thresh value that measures linear dependence - of a newly formed column with the existing columns. Defaults to 1e-6. - - In this version the generated matrix distribution *is* uniform over the - manifold O(n) w.r.t. the induced R^(n^2) Lebesgue measure,at a slight - computational overhead (randn + normalization, as opposed to rand ). - - (c) Ofek Shilon , 2006. - - Parameters - ========== - n integer - matrix size - - Returns - ======= - numpy.ndarray n x n - Random orthogonal real matrix - """ - - M = np.zeros((n, n)) - - # Gram-schmidt on random column vectors - vi = np.random.rand(n, 1) - # the n-dimensional normal distribution has spherical symmetry, which implies - # that after normalization the drawn vectors would be uniformly distributed on the - # n-dimensional unit sphere. - M[:, 0] = (vi * 1 / (np.linalg.norm(vi))).T - - for i in range(1, n): - nrm = 0 - while nrm < tol: - vi = np.random.rand(n, 1) - vi = vi - M[:, 0:i] @ (M[:, 0:i].T @ vi) - nrm = np.linalg.norm(vi) - M[:, i] = (vi / nrm).T - - return (M) - - def gauss2cubature(state, alpha=1.0): r"""Evaluate the cubature points for an input Gaussian state. This is done under the assumption that the input state is :math:`\mathcal{N}(\mathbf{\mu}, \Sigma)` of dimension :math:`n`. We From 2a5027925b4d18cb1adc47cc90a4b7d6c30dc9e7 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Thu, 3 Oct 2024 10:29:35 +0100 Subject: [PATCH 25/51] Remove unused `_time_interval` variable in UKF and SIF --- stonesoup/predictor/kalman.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 2763437e5..04d74e9b4 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -313,11 +313,6 @@ class UnscentedKalmanPredictor(KalmanPredictor): doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns") - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - - self._time_interval = None - def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions for the unscented transform @@ -590,11 +585,6 @@ class StochasticIntegrationPredictor(KalmanPredictor): default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - - self._time_interval = None - def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions for the unscented transform From 87275f00800a7324bfc5f8b12bfe2faaf55bc845 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Thu, 3 Oct 2024 10:31:32 +0100 Subject: [PATCH 26/51] Minor changes to SIF to improve clarity of iteration stopping criteria --- stonesoup/predictor/kalman.py | 5 ++--- stonesoup/smoother/kalman.py | 2 +- stonesoup/updater/kalman.py | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 04d74e9b4..43cc45e11 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -651,8 +651,7 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # - SIR recursion for state predictive moments computation # -- until either max iterations or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, - np.any([(np.linalg.norm(Vx) > self.Eps),]),]): + while N < self.Nmin or (N < self.Nmax and np.linalg.norm(Vx) > self.Eps): N += 1 # -- cubature points and weights computation (for standard normal PDF) @@ -675,7 +674,7 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): N = 0 # - SIR recursion for state predictive moments computation # -- until max iterations are reached or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(VPx) > self.Eps)])]): + while N < self.Nmin or (N < self.Nmax and np.linalg.norm(VPx) > self.Eps): N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index c1a88f330..d00cdd036 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -383,7 +383,7 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # - SIR recursion for measurement predictive moments computation # -- until either required number of iterations is reached or threshold is reached - while N < self.Nmin or all([N < self.Nmax, (np.linalg.norm(VPxx) > self.Eps)]): + while N < self.Nmin or (N < self.Nmax and np.linalg.norm(VPxx) > self.Eps): N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 8f719beeb..6518c78a6 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -970,7 +970,7 @@ def predict_measurement( N = 0 # SIR recursion for measurement predictive moments computation # until either number of iterations is reached or threshold is reached - while N < self.Nmin or np.all([N < self.Nmax, np.any([(np.linalg.norm(Vz) > self.Eps)])]): + while N < self.Nmin or (N < self.Nmax and np.linalg.norm(Vz) > self.Eps): N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix From 7b7db22b8ea5ca898f81343821e06762923d35e2 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Thu, 3 Oct 2024 10:35:06 +0100 Subject: [PATCH 27/51] Remove print statements in state --- stonesoup/types/state.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/stonesoup/types/state.py b/stonesoup/types/state.py index a57b45e51..64272349e 100644 --- a/stonesoup/types/state.py +++ b/stonesoup/types/state.py @@ -1063,9 +1063,6 @@ def mean(self): @clearable_cached_property('state_vector') def covar(self): """Sample covariance matrix for ensemble""" - print(type(self.state_vector)) - print(self.ndim) - print(self.num_vectors) return np.cov(self.state_vector) @clearable_cached_property('state_vector') From da0f9a29514f7a724fab9ec9bef991f5f49cc1c4 Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Thu, 3 Oct 2024 10:40:01 +0100 Subject: [PATCH 28/51] Add test for SIF invalid order --- stonesoup/functions/tests/test_functions.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index f45c2735e..79b4ec111 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -406,3 +406,8 @@ def test_stochastic_integration(order, nx): # Check if off-diagonal elements are close to 0 off_diagonal_elements = var[~np.eye(nx, dtype=bool)] assert np.allclose(off_diagonal_elements, 0, atol=1e-5) + + +def test_stochastic_integration_invalid_order(): + with pytest.raises(ValueError, match="This order of SIF is not supported"): + stochasticCubatureRulePoints(5, 2) From 9a7a4d38ca292d5a7c762a98bc0a995b75bf836d Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Thu, 3 Oct 2024 10:57:21 +0100 Subject: [PATCH 29/51] Take advantage of vectorised functions in subPointsAndTransfer --- stonesoup/functions/__init__.py | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 780c6a181..0bff481ec 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1085,19 +1085,11 @@ def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, order) # -- points transformation for given filtering mean and covariance matrix - points = sqrtCov @ SCRSigmaPoints + mean + points = StateVectors(sqrtCov@SCRSigmaPoints + mean) # -- points transformation through the function - sigma_points_states = [] - for point in points.T: - state_copy = copy.copy(state) - state_copy.state_vector = StateVector(point) - sigma_points_states.append(state_copy) - trsfPoints = StateVectors( - [ - transFunct(sigma_points_state) - for sigma_points_state in sigma_points_states - ] - ) + state_copy = copy.copy(state) + state_copy.state_vector = points + trsfPoints = transFunct(state_copy) return points, w, trsfPoints From 11f2e01eacddc626250b563a274bbaf60b21ca49 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Mon, 4 Nov 2024 14:43:15 -0500 Subject: [PATCH 30/51] Update stonesoup/predictor/kalman.py Fixed typo in Stochastic Integration predictor docstring. Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/predictor/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 43cc45e11..bffc968a5 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -19,7 +19,7 @@ class KalmanPredictor(Predictor): r"""A predictor class which forms the basis for the family of Kalman - predictors. This class also servas the (specific) Kalman Filter + predictors. This class also serves as the (specific) Kalman Filter :class:`~.Predictor` class. Here transition and control models must be linear: .. math:: From f4bba6d638d85f639809f08b8d6df03754347437 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Mon, 4 Nov 2024 14:44:42 -0500 Subject: [PATCH 31/51] Update stonesoup/smoother/kalman.py Fixed typo in Stochastic Integration Smoother Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/smoother/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index d00cdd036..6ef3daf58 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -357,7 +357,7 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): Returns ------- - : Matrix + : :class:`numpy.ndarray` The smoothing gain """ From 1bdba7f6a9522ae087716d194c975b91dbec7a54 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Mon, 4 Nov 2024 14:45:53 -0500 Subject: [PATCH 32/51] Update stonesoup/functions/__init__.py Fixed typo in Stochastic Cubature Rule function comment Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/functions/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 0bff481ec..b28d8defd 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -959,7 +959,7 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. def stochasticCubatureRulePoints(nx, order): - """Stochstic cubature rule points + """Stochastic cubature rule points computation of cubature points and weights for the stochastic integration From 0efee8de72217130866038f5bdcccb320d7948a0 Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 4 Nov 2024 16:34:07 -0500 Subject: [PATCH 33/51] Changed several properties from float to int in SIF code. --- stonesoup/predictor/kalman.py | 6 +++--- stonesoup/smoother/kalman.py | 6 +++--- stonesoup/updater/kalman.py | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index bffc968a5..ab4f7e940 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -575,13 +575,13 @@ class StochasticIntegrationPredictor(KalmanPredictor): doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.", ) - Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") - Nmin: float = Property( + Nmax: int = Property(default=10, doc="maximal number of iterations of SIR") + Nmin: int = Property( default=5, doc="minimal number of iterations of stochastic integration rule (SIR)", ) Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") - SIorder: float = Property( + SIorder: int = Property( default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 6ef3daf58..d39623781 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -335,13 +335,13 @@ class StochasticIntegrationSmoother(KalmanSmoother): transition_model: TransitionModel = Property(doc="The transition model to be used.") - Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") - Nmin: float = Property( + Nmax: int = Property(default=10, doc="maximal number of iterations of SIR") + Nmin: int = Property( default=5, doc="minimal number of iterations of stochastic integration rule (SIR)", ) Eps: float = Property(default=5e-3, doc="allowed threshold for integration error") - SIorder: float = Property( + SIorder: int = Property( default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 6518c78a6..e5efdca60 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -921,13 +921,13 @@ class StochasticIntegrationUpdater(KalmanUpdater): "specified on construction, or in the measurement, then error " "will be thrown.", ) - Nmax: float = Property(default=10, doc="maximal number of iterations of SIR") - Nmin: float = Property( + Nmax: int = Property(default=10, doc="maximal number of iterations of SIR") + Nmin: int = Property( default=5, doc="minimal number of iterations of stochastic integration rule (SIR)", ) Eps: float = Property(default=5e-4, doc="allowed threshold for integration error") - SIorder: float = Property( + SIorder: int = Property( default=5, doc="order of SIR (orders 1, 3, 5 are currently supported)" ) From 8fcc7128617993e21218d0507a4bc2bde35a3d02 Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 4 Nov 2024 16:43:44 -0500 Subject: [PATCH 34/51] Added description to SIF measurement update method --- stonesoup/updater/kalman.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index e5efdca60..4dcf9e3b0 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -935,7 +935,8 @@ class StochasticIntegrationUpdater(KalmanUpdater): def predict_measurement( self, predicted_state, measurement_model=None, measurement_noise=True, **kwargs ): - """SIF. + """Stochastic Integration Filter measurement prediction step. Uses + stochastic integration to estimate a Gaussian distributed predicted measurement. Parameters ---------- From 0c410643f234deeaadea29c0376ee5a5b5266ae7 Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 4 Nov 2024 17:16:38 -0500 Subject: [PATCH 35/51] Revised docstrings in SIF functions --- stonesoup/functions/__init__.py | 47 ++++++++++++++++++--------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index 636ef0a59..e333159ab 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -964,17 +964,19 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. def stochasticCubatureRulePoints(nx, order): """Stochastic cubature rule points - computation of cubature points and weights for the stochastic integration + Computation of cubature points and weights for the stochastic integration. Parameters ========== - integer - number of points + nx : int + Number of points, presumably equivilant to state dimension. + order : int + Order for stochastic integration. Only orders 1, 3, and 5 are supported. Returns ======= - numpy.ndarray dim x nx - Matrix of sigma points + (numpy.ndarray, numpy.ndarray) + Tuple of sigma points and weights """ if order == 1: @@ -1061,26 +1063,27 @@ def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): Parameters ========== - nx : integer - dimension - order : integer - order of SIF rule - sqrtCov : array of float 64 [nx x nx] - square root of covariance - mean : array of int 64 [nx x 1] - mean - transFunct : function - function to transfer state vectors - state : state - whole state + nx : int + Dimension for cubature points, equivilant to state dimension. + order : int + Order for Stochastic Integration. Only orders 1, 3, and 5 are supported + sqrtCov : np.ndarray + Matrix square root array of shape (nx, nx) of the covariance matrix + mean : np.ndarray + An array of shape (nx, 1) of the state mean + transFunct : Callable + A function to transfer state vectors + state : :class:`~.State` + State object used to save function output to Returns ======= - points : numpy.ndarray nx x number of points (based on order and dim) - cubature points - w : numpy.ndarray number of points x - weights - trsfPoints : numpy.ndarray nx x number of points (based on order and dim) + points : numpy.ndarray + Array of shape (nx, number of points) of cubature points + w : numpy.ndarray + Array of shape (number of points) of weights + trsfPoints : numpy.ndarray + Array of shape (nx, number of points) (based on order and dim) of cubature transformed points """ From fa8f0299dd270c5c66eaba1de51614672347993d Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Wed, 13 Nov 2024 10:42:50 -0500 Subject: [PATCH 36/51] Update stonesoup/updater/kalman.py Revised SIF updater docstring Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/updater/kalman.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 4dcf9e3b0..a2c1ecdcc 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -948,6 +948,8 @@ def predict_measurement( dependent on the received measurement (the default is `None`, in which case the updater will use the measurement model specified on initialisation) + measurement_noise : bool + Include measurement noise or not Returns ------- From 37172f2dcf80fc4fd41bc45b65ccfafe69b4bb6b Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Wed, 13 Nov 2024 10:43:24 -0500 Subject: [PATCH 37/51] Update stonesoup/predictor/kalman.py fixed typo in SIF predictor docstring Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/predictor/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index ab4f7e940..64684cfb5 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -591,7 +591,7 @@ def _transition_and_control_function(self, prior_state, **kwargs): Parameters ---------- - prior_state_vector : :class:`~.State` + prior_state : :class:`~.State` Prior state vector **kwargs : various, optional These are passed to :class:`~.TransitionModel.function` From aa7fb38416e9cb134d49c9efba083d46869265ea Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Wed, 13 Nov 2024 10:44:44 -0500 Subject: [PATCH 38/51] Update stonesoup/functions/__init__.py Fixed typo on line 1067 of SIF built in function Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/functions/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index e333159ab..c2d8321b3 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1064,7 +1064,7 @@ def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): Parameters ========== nx : int - Dimension for cubature points, equivilant to state dimension. + Dimension for cubature points, equivalent to state dimension. order : int Order for Stochastic Integration. Only orders 1, 3, and 5 are supported sqrtCov : np.ndarray From 5ec00afca1b9950c608c66f49aaff6b53ac5b44c Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Wed, 13 Nov 2024 10:45:03 -0500 Subject: [PATCH 39/51] Update stonesoup/functions/__init__.py Fixed typo on line 972 of SIF built in function Co-authored-by: Henry Pritchett <87075245+hpritchett-dstl@users.noreply.github.com> --- stonesoup/functions/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index c2d8321b3..fd04e1724 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -969,7 +969,7 @@ def stochasticCubatureRulePoints(nx, order): Parameters ========== nx : int - Number of points, presumably equivilant to state dimension. + Number of points, presumably equivalent to state dimension. order : int Order for stochastic integration. Only orders 1, 3, and 5 are supported. From 056c27da8707ad050e8cbc4aa31cdf6176efcec2 Mon Sep 17 00:00:00 2001 From: Osmium Date: Wed, 13 Nov 2024 10:50:04 -0500 Subject: [PATCH 40/51] Renamed cubPointsAndTransfer function to cub_points_and_tf --- stonesoup/functions/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index e333159ab..15e4bb563 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -1057,7 +1057,7 @@ def stochasticCubatureRulePoints(nx, order): return (SCRSigmaPoints, weights) -def cubPointsAndTransfer(nx, order, sqrtCov, mean, transFunct, state): +def cub_points_and_tf(nx, order, sqrtCov, mean, transFunct, state): r""" Calculates cubature points for stochastic integration filter and puts them through given function (measurement/dynamics) From c421c146a0681d5935843ec5390e4d947d3b829c Mon Sep 17 00:00:00 2001 From: Osmium Date: Wed, 13 Nov 2024 10:59:46 -0500 Subject: [PATCH 41/51] Added mention of time_interval argument to SIF smoother docstring --- stonesoup/smoother/kalman.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index d39623781..2110caf86 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -354,11 +354,13 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): The input state prediction : :class:`~.GaussianStatePrediction` The prediction from the subsequent timestep + time_interval: :class:`datetime.time_delta` + time interval of the prediction is needed to propagate the states Returns ------- - : :class:`numpy.ndarray` - The smoothing gain + : :class:`numpy.ndarray` + The smoothing gain """ From 4e14a10dc7620a3779234fcd814ca616bfcebab0 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:04:01 -0500 Subject: [PATCH 42/51] Update kalman.py Corrected import statment in kalman smoother file pertaining to SIF --- stonesoup/smoother/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 2110caf86..93d310f2e 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -4,7 +4,7 @@ import numpy as np from ..base import Property -from ..functions import (gauss2sigma, unscented_transform, cubPointsAndTransfer) +from ..functions import (gauss2sigma, unscented_transform, cub_points_and_tf) from ..models.base import LinearModel from ..models.transition.base import TransitionModel from ..models.transition.linear import LinearGaussianTransitionModel From 06f08580514614995b1d9928435182243a5b279e Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:04:49 -0500 Subject: [PATCH 43/51] Update kalman.py Corrected import statment in kalman predictor file pertaining to SIF --- stonesoup/predictor/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 64684cfb5..d4c11ac4f 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -14,7 +14,7 @@ from ..models.control import ControlModel from ..models.control.linear import LinearControlModel from ..functions import (gauss2sigma, unscented_transform, cubature_transform, - cubPointsAndTransfer) + cub_points_and_tf) class KalmanPredictor(Predictor): From b83e7647908820c902fcfba48d3b086daa013d69 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:05:27 -0500 Subject: [PATCH 44/51] Update kalman.py Corrected import statment in kalman updater file pertaining to SIF --- stonesoup/updater/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index a2c1ecdcc..63bcd77a5 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -13,7 +13,7 @@ from ..models.measurement.linear import LinearGaussian from ..models.measurement import MeasurementModel from ..functions import (gauss2sigma, unscented_transform, cubature_transform, - cubPointsAndTransfer) + cub_points_and_tf) from ..measures import Measure, Euclidean From cd3e782acd0e8c7c775534deb1507c2760f49494 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:07:38 -0500 Subject: [PATCH 45/51] Update kalman.py Fixed incorrect function name in SIF Smoother --- stonesoup/smoother/kalman.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index 93d310f2e..fb3960e33 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -389,7 +389,7 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sf, + xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sf, efMean, transition_function, prediction) From 2e0da5af439512c6f9e2bc13635eaa73ab2dd26d Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:08:35 -0500 Subject: [PATCH 46/51] Update kalman.py Fixed incorrect function name in SIF predictor --- stonesoup/predictor/kalman.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index d4c11ac4f..6b73de852 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -656,7 +656,7 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) @@ -678,7 +678,7 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, fpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sp, prior.mean, transition_and_control_function, prior) From af58a3265a088cae4bf497a8b4174cb5edd7b631 Mon Sep 17 00:00:00 2001 From: John Hiles <46450062+0sm1um@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:09:15 -0500 Subject: [PATCH 47/51] Update kalman.py Fixed incorrect function name in SIF Updater --- stonesoup/updater/kalman.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 63bcd77a5..8c14e4c4c 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -977,7 +977,7 @@ def predict_measurement( N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + xpoints, w, hpoints = cub_points_and_tf(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) @@ -999,7 +999,7 @@ def predict_measurement( N += 1 # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix - xpoints, w, hpoints = cubPointsAndTransfer(nx, self.SIorder, Sp, + xpoints, w, hpoints = cub_points_and_tf(nx, self.SIorder, Sp, epMean, measurement_model.function, predicted_state) From 388949b1566e65a767bf4bd10ec2e07c7b4572e5 Mon Sep 17 00:00:00 2001 From: Osmium Date: Thu, 14 Nov 2024 23:15:03 -0500 Subject: [PATCH 48/51] Indentation fixes in SIF predictor, smoother, and updater --- stonesoup/predictor/kalman.py | 12 ++++++------ stonesoup/smoother/kalman.py | 6 +++--- stonesoup/updater/kalman.py | 6 +++--- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/stonesoup/predictor/kalman.py b/stonesoup/predictor/kalman.py index 6b73de852..0c141346b 100644 --- a/stonesoup/predictor/kalman.py +++ b/stonesoup/predictor/kalman.py @@ -657,9 +657,9 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sp, - prior.mean, - transition_and_control_function, - prior) + prior.mean, + transition_and_control_function, + prior) # Stochastic integration rule for predictive state mean and # covariance matrix SumRx = np.average(fpoints, axis=1, weights=w) @@ -679,9 +679,9 @@ def predict(self, prior, timestamp=None, control_input=None, **kwargs): # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sp, - prior.mean, - transition_and_control_function, - prior) + prior.mean, + transition_and_control_function, + prior) # -- stochastic integration rule for predictive state mean and covariance # matrix diff --git a/stonesoup/smoother/kalman.py b/stonesoup/smoother/kalman.py index fb3960e33..49536a714 100644 --- a/stonesoup/smoother/kalman.py +++ b/stonesoup/smoother/kalman.py @@ -390,9 +390,9 @@ def _smooth_gain(self, state, prediction, time_interval, **kwargs): # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, fpoints = cub_points_and_tf(nx, self.SIorder, Sf, - efMean, - transition_function, - prediction) + efMean, + transition_function, + prediction) # Stochastic integration rule for predictive measurement # mean and covariance matrix fpoints_diff = fpoints - predMean diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 8c14e4c4c..973d21a3d 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -978,9 +978,9 @@ def predict_measurement( # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, hpoints = cub_points_and_tf(nx, self.SIorder, Sp, - epMean, - measurement_model.function, - predicted_state) + epMean, + measurement_model.function, + predicted_state) # -- stochastic integration rule for predictive measurement mean and covariance # matrix and predictive state and measurement covariance matrix SumRz = np.average(hpoints, axis=1, weights=w) From 8e1b6d5cd7475d814103e32bc86fe6ed293f526a Mon Sep 17 00:00:00 2001 From: Osmium Date: Thu, 14 Nov 2024 23:22:44 -0500 Subject: [PATCH 49/51] Fixed whitespace flake8 violation with SIF Updater --- stonesoup/updater/kalman.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/stonesoup/updater/kalman.py b/stonesoup/updater/kalman.py index 973d21a3d..eaafd5d3b 100644 --- a/stonesoup/updater/kalman.py +++ b/stonesoup/updater/kalman.py @@ -1000,9 +1000,9 @@ def predict_measurement( # -- cubature points and weights computation (for standard normal PDF) # -- points transformation for given filtering mean and covariance matrix xpoints, w, hpoints = cub_points_and_tf(nx, self.SIorder, Sp, - epMean, - measurement_model.function, - predicted_state) + epMean, + measurement_model.function, + predicted_state) # Stochastic integration rule for predictive measurement mean and covariance # Matrix and predictive state and measurement covariance matrix hpoints_diff = hpoints - zp From 511eca9eb2dd2c20dd73850a3ed09377a69ac32c Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 2 Dec 2024 19:10:21 -0500 Subject: [PATCH 50/51] Renamed SIF function to conform with style guidelines --- stonesoup/functions/__init__.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/stonesoup/functions/__init__.py b/stonesoup/functions/__init__.py index d00668414..23704762c 100644 --- a/stonesoup/functions/__init__.py +++ b/stonesoup/functions/__init__.py @@ -961,10 +961,8 @@ def cubature_transform(state, fun, points_noise=None, covar_noise=None, alpha=1. return mean, covar, cross_covar, cubature_points_t -def stochasticCubatureRulePoints(nx, order): - """Stochastic cubature rule points - - Computation of cubature points and weights for the stochastic integration. +def stochastic_cubature_rule_points(nx, order): + """Computation of cubature points and weights for the stochastic integration. Parameters ========== @@ -1088,7 +1086,7 @@ def cub_points_and_tf(nx, order, sqrtCov, mean, transFunct, state): """ # -- cubature points and weights computation (for standard normal PDF) - SCRSigmaPoints, w = stochasticCubatureRulePoints(nx, order) + SCRSigmaPoints, w = stochastic_cubature_rule_points(nx, order) # -- points transformation for given filtering mean and covariance matrix points = StateVectors(sqrtCov@SCRSigmaPoints + mean) From f6929a0ff6c80e76f7b29e22c3232fe41be7d15e Mon Sep 17 00:00:00 2001 From: Osmium Date: Mon, 2 Dec 2024 19:16:01 -0500 Subject: [PATCH 51/51] Updated unit tests for stochastic integration filter to reflect new function names. --- stonesoup/functions/tests/test_functions.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/stonesoup/functions/tests/test_functions.py b/stonesoup/functions/tests/test_functions.py index 79b4ec111..79581de30 100644 --- a/stonesoup/functions/tests/test_functions.py +++ b/stonesoup/functions/tests/test_functions.py @@ -7,7 +7,7 @@ from .. import ( cholesky_eps, jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma, rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart, dotproduct, gm_sample, - gauss2cubature, cubature2gauss, cubature_transform, stochasticCubatureRulePoints) + gauss2cubature, cubature2gauss, cubature_transform, stochastic_cubature_rule_points) from ...types.array import StateVector, StateVectors, Matrix, CovarianceMatrix from ...types.state import State, GaussianState @@ -391,7 +391,7 @@ def identity_function(inpu): ] ) def test_stochastic_integration(order, nx): - points, weights = stochasticCubatureRulePoints(nx, order) + points, weights = stochastic_cubature_rule_points(nx, order) # Mean assert np.allclose(np.average(points, weights=weights, axis=1), 0, atol=1e-5) @@ -410,4 +410,4 @@ def test_stochastic_integration(order, nx): def test_stochastic_integration_invalid_order(): with pytest.raises(ValueError, match="This order of SIF is not supported"): - stochasticCubatureRulePoints(5, 2) + stochastic_cubature_rule_points(5, 2)