# Path: imusim/maths/transforms.py
# class UnscentedTransform(object):
#     """
#     Implementation of the Unscented Transform of Julier and Uhlmann.
# 
#     The unscented transform propagates mean and covariance information
#     through a non-linear function. An instance of this class is callable
#     to apply the function to a vector of mean values and a covariance
#     matrix, returning an output mean vector and covariance matrix.
#     """
# 
#     _sum = functools.partial(np.sum, axis=0)
# 
#     def __init__(self, function):
#         """
#         Construct unscented transform.
# 
#         @param function: The function to apply.
#         """
#         assert isinstance(function, collections.Callable)
#         self._function = function
# 
#     def __call__(self, mean, covariance, *args, **kwargs):
#         """
#         Propagate mean and covariance values through the transform function.
# 
#         Additional arguments are passed through to the function.
# 
#         @param mean: Nx1 column vector of mean input values.
#         @param covariance: NxN covariance of input values.
#         @param returnSigmas: If True, return sigma points and weights.
# 
#         @return: Mx1 column vector of mean output values, MxM covariance
#             of output values. If returnSigmas is True, additionally a list of
#             input sigma point column vectors, a list of output sigma point
#             column vectors, and a list of sigma point weights.
#         """
#         returnSigmas = kwargs.pop('returnSigmas', False)
#         inputSigmaPoints, weights = UnscentedTransform.sigmaPoints(mean, covariance)
#         outputSigmaPoints = [self._function(p, *args, **kwargs) for p in inputSigmaPoints]
#         weightedOutputPoints = zip(weights, outputSigmaPoints)
#         outputMean = self._sum(W*y for W,y in weightedOutputPoints)
#         outputCovariance = self._sum(W*(y-outputMean)*(y-outputMean).T for W,y in weightedOutputPoints)
#         if returnSigmas:
#             return outputMean, outputCovariance, inputSigmaPoints, outputSigmaPoints, weights
#         else:
#             return outputMean, outputCovariance
# 
#     @staticmethod
#     def sigmaPoints(mean, covariance):
#         """
#         Generate sigma points around the given mean values based on covariance.
# 
#         Implements the symmetric sigma point set of Julier and Uhlmann 2004,
#         'Unscented Filtering and Nonlinear Estimation', equation 12. Weighting
#         applied to first sigma point is 1/3 based on the assumption of Gaussian
#         distributions.
# 
#         @return: List of sigma point column vectors, list of weights.
#         """
#         N = len(mean)
#         mean = np.reshape(mean, (N,1))
#         assert covariance.shape == (N,N)
# 
#         sigmaPoints = [mean] * (2*N + 1)
#         w0 = 1/3 # based on assumption of Gaussian distributions.
# 
#         cholesky = linalg.cholesky((N/(1-w0)) * covariance)
#         # cholesky returns A s.t. A*A.T = P so we use the columns of A
#         columns = np.hsplit(cholesky, N)
#         for i, column in enumerate(columns):
#             sigmaPoints[i+1] = mean + column
#             sigmaPoints[i+1+N] = mean - column
#         weights = [w0] + [(1-w0)/(2*N)] * (2 * N)
#         return sigmaPoints, weights


import numpy as np
from imusim.maths.transforms import UnscentedTransform
from numpy.linalg import inv
from itertools import izip
            measurement vector. N and M are the numbers of states and measurement
            outputs.
        @param state: Initial state estimate.
        @param stateCovariance: Initial state covariance matrix.
        @param processCovariance: Process noise covariance matrix.
        @param measurementCovariance: Measurement noise covariance matrix.
        """

        self.stateUpdateFunction = stateUpdateFunction
        self.measurementFunction = measurementFunction

        self.state = state
        self.stateCovariance = stateCovariance
        self.processCovariance = processCovariance
        self.measurementCovariance = measurementCovariance

    @property
    def stateUpdateFunction(self):
        return self._stateUpdateUT._function

    @stateUpdateFunction.setter
    def stateUpdateFunction(self, function):
        self._stateUpdateUT = UnscentedTransform(function)

    @property
    def measurementFunction(self):
        return self._measurementUT._function

    @measurementFunction.setter
    def measurementFunction(self, function):

# Path: imusim/maths/transforms.py
# class UnscentedTransform(object):
#     """
#     Implementation of the Unscented Transform of Julier and Uhlmann.
# 
#     The unscented transform propagates mean and covariance information
#     through a non-linear function. An instance of this class is callable
#     to apply the function to a vector of mean values and a covariance
#     matrix, returning an output mean vector and covariance matrix.
#     """
# 
#     _sum = functools.partial(np.sum, axis=0)
# 
#     def __init__(self, function):
#         """
#         Construct unscented transform.
# 
#         @param function: The function to apply.
#         """
#         assert isinstance(function, collections.Callable)
#         self._function = function
# 
#     def __call__(self, mean, covariance, *args, **kwargs):
#         """
#         Propagate mean and covariance values through the transform function.
# 
#         Additional arguments are passed through to the function.
# 
#         @param mean: Nx1 column vector of mean input values.
#         @param covariance: NxN covariance of input values.
#         @param returnSigmas: If True, return sigma points and weights.
# 
#         @return: Mx1 column vector of mean output values, MxM covariance
#             of output values. If returnSigmas is True, additionally a list of
#             input sigma point column vectors, a list of output sigma point
#             column vectors, and a list of sigma point weights.
#         """
#         returnSigmas = kwargs.pop('returnSigmas', False)
#         inputSigmaPoints, weights = UnscentedTransform.sigmaPoints(mean, covariance)
#         outputSigmaPoints = [self._function(p, *args, **kwargs) for p in inputSigmaPoints]
#         weightedOutputPoints = zip(weights, outputSigmaPoints)
#         outputMean = self._sum(W*y for W,y in weightedOutputPoints)
#         outputCovariance = self._sum(W*(y-outputMean)*(y-outputMean).T for W,y in weightedOutputPoints)
#         if returnSigmas:
#             return outputMean, outputCovariance, inputSigmaPoints, outputSigmaPoints, weights
#         else:
#             return outputMean, outputCovariance
# 
#     @staticmethod
#     def sigmaPoints(mean, covariance):
#         """
#         Generate sigma points around the given mean values based on covariance.
# 
#         Implements the symmetric sigma point set of Julier and Uhlmann 2004,
#         'Unscented Filtering and Nonlinear Estimation', equation 12. Weighting
#         applied to first sigma point is 1/3 based on the assumption of Gaussian
#         distributions.
# 
#         @return: List of sigma point column vectors, list of weights.
#         """
#         N = len(mean)
#         mean = np.reshape(mean, (N,1))
#         assert covariance.shape == (N,N)
# 
#         sigmaPoints = [mean] * (2*N + 1)
#         w0 = 1/3 # based on assumption of Gaussian distributions.
# 
#         cholesky = linalg.cholesky((N/(1-w0)) * covariance)
#         # cholesky returns A s.t. A*A.T = P so we use the columns of A
#         columns = np.hsplit(cholesky, N)
#         for i, column in enumerate(columns):
#             sigmaPoints[i+1] = mean + column
#             sigmaPoints[i+1+N] = mean - column
#         weights = [w0] + [(1-w0)/(2*N)] * (2 * N)
#         return sigmaPoints, weights


# Path: imusim/maths/kalman.py
import numpy as np
from imusim.maths.transforms import UnscentedTransform
from numpy.linalg import inv
from itertools import izip

            measurement vector. N and M are the numbers of states and measurement
            outputs.
        @param state: Initial state estimate.
        @param stateCovariance: Initial state covariance matrix.
        @param processCovariance: Process noise covariance matrix.
        @param measurementCovariance: Measurement noise covariance matrix.
        """

        self.stateUpdateFunction = stateUpdateFunction
        self.measurementFunction = measurementFunction

        self.state = state
        self.stateCovariance = stateCovariance
        self.processCovariance = processCovariance
        self.measurementCovariance = measurementCovariance

    @property
    def stateUpdateFunction(self):
        return self._stateUpdateUT._function

    @stateUpdateFunction.setter
    def stateUpdateFunction(self, function):
        self._stateUpdateUT = UnscentedTransform(function)

    @property
    def measurementFunction(self):
        return self._measurementUT._function

    @measurementFunction.setter
    def measurementFunction(self, function):

        self._measurementUT = UnscentedTransform(function)