# 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
        @param stateUpdateFunction: Non-linear function to propagate state into
            the next timestep. The function is called with two arguments, the
            current Nx1 state vector and the current Mx1 control input vector.
            It should return a new Nx1 state vector. N and M are the numbers of
            states and control inputs.
        @param measurementFunction: Non-linear function relating state
            variables to expected measurements. The function is called with the
            current Nx1 state vector as an argument, and should return an Mx1
            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):

# 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

        @param stateUpdateFunction: Non-linear function to propagate state into
            the next timestep. The function is called with two arguments, the
            current Nx1 state vector and the current Mx1 control input vector.
            It should return a new Nx1 state vector. N and M are the numbers of
            states and control inputs.
        @param measurementFunction: Non-linear function relating state
            variables to expected measurements. The function is called with the
            current Nx1 state vector as an argument, and should return an Mx1
            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)