# Source code for filterpy.leastsq.least_squares

    # -*- coding: utf-8 -*-
    # pylint: disable=C0103, R0913, R0902, C0326, R0914
    # disable snake_case warning, too many arguments, too many attributes,
    # one space before assignment, too many local variables

    """Copyright 2015 Roger R Labbe Jr.

    FilterPy library.
    http://github.com/rlabbe/filterpy

    Documentation at:
    https://filterpy.readthedocs.org

    Supporting book at:
    https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

    This is licensed under an MIT license. See the readme.MD file
    for more information.
    """

    from __future__ import absolute_import, division
    from math import sqrt
    import numpy as np
    from filterpy.kalman import pretty_str

    class LeastSquaresFilter(object):
        """Implements a Least Squares recursive filter. Formulation is per
        Zarchan [1]_.

        Filter may be of order 0 to 2. Order 0 assumes the value being tracked is
        a constant, order 1 assumes that it moves in a line, and order 2 assumes
        that it is tracking a second order polynomial.

        Parameters
        ----------

        dt : float
           time step per update

        order : int
            order of filter 0..2

        noise_sigma : float
            sigma (std dev) in x. This allows us to calculate the error of
            the filter, it does not influence the filter output.

        Attributes
        ----------

        n : int
            step in the recursion. 0 prior to first call, 1 after the first call,
            etc.

        K : np.array
            Gains for the filter. K[0] for all orders, K[1] for orders 0 and 1, and
            K[2] for order 2

        x: np.array (order + 1, 1)
            estimate(s) of the output. It is a vector containing the estimate x
            and the derivatives of x: [x x' x''].T. It contains as many
            derivatives as the order allows. That is, a zero order filter has
            no derivatives, a first order has one derivative, and a second order
            has two.

        y : float
            residual (difference between measurement projection of previous
            estimate to current time).

        Examples
        --------

        .. code-block:: Python

            from filterpy.leastsq import LeastSquaresFilter

            lsq = LeastSquaresFilter(dt=0.1, order=1, noise_sigma=2.3)

            while True:
                z = sensor_reading()  # get a measurement
                x = lsq.update(z)     # get the filtered estimate.
                print('error: {}, velocity error: {}'.format(
                      lsq.error, lsq.derror))

        References
        ----------

        .. [1] Zarchan and Musoff. "Fundamentals of Kalman Filtering: A Practical
              Approach." Third Edition. AIAA, 2009.
        """

        def __init__(self, dt, order, noise_sigma=0.):
            if order < 0 or order > 2:
                raise ValueError('order must be between 0 and 2')

            self.dt = dt

            self.sigma = noise_sigma
            self._order = order

            self.reset()

        def reset(self):
            """ reset filter back to state at time of construction"""

            self.n = 0  # nth step in the recursion
            self.x = np.zeros(self._order + 1)
            self.K = np.zeros(self._order + 1)
            self.y = 0  # residual

        def update(self, z):
            """ Update filter with new measurement `z`

            Returns
            -------

            x : np.array
                estimate for this time step (same as self.x)
            """

            self.n += 1
            # rename for readability
            n = self.n
            dt = self.dt
            x = self.x
            K = self.K
            y = self.y

            if self._order == 0:
                K[0] = 1. / n
                y = z - x
                x[0] += K[0] * y

            elif self._order == 1:
                K[0] = 2. * (2*n - 1) / (n*(n + 1))
                K[1] = 6. / (n*(n + 1)*dt)

                y = z - x[0] - (dt * x[1])

                x[0] += (K[0] * y) + (dt * x[1])
                x[1] += (K[1] * y)

            else:
                den = n * (n+1) * (n+2)
                K[0] = 3. * (3*n**2 - 3*n + 2) / den
                K[1] = 18. * (2*n-1) / (den*dt)
                K[2] = 60. / (den*dt**2)

                y = z - x[0] - (dt * x[1]) - (0.5 * dt**2 * x[2])

                x[0] += (K[0] * y) + (x[1] * dt) + (.5 * dt**2 * x[2])
                x[1] += (K[1] * y) + (x[2] * dt)
                x[2] += (K[2] * y)
            return self.x

        def errors(self):
            """
            Computes and returns the error and standard deviation of the
            filter at this time step.

            Returns
            -------

            error : np.array size 1xorder+1
            std : np.array size 1xorder+1
            """

            n = self.n
            dt = self.dt
            order = self._order
            sigma = self.sigma

            error = np.zeros(order + 1)
            std = np.zeros(order + 1)

            if n == 0:
                return (error, std)

            if order == 0:
                error[0] = sigma/sqrt(n)
                std[0] = sigma/sqrt(n)

            elif order == 1:
                if n > 1:
                    error[0] = sigma * sqrt(2*(2*n-1) / (n*(n+1)))
                    error[1] = sigma * sqrt(12. / (n*(n*n-1)*dt*dt))
                std[0] = sigma * sqrt((2*(2*n-1)) / (n*(n+1)))
                std[1] = (sigma/dt) * sqrt(12. / (n*(n*n-1)))

            elif order == 2:
                dt2 = dt * dt

                if n >= 3:
                    error[0] = sigma * sqrt(3*(3*n*n-3*n+2) / (n*(n+1)*(n+2)))
                    error[1] = sigma * sqrt(12*(16*n*n-30*n+11) /
                                            (n*(n*n-1)*(n*n-4)*dt2))
                    error[2] = sigma * sqrt(720/(n*(n*n-1)*(n*n-4)*dt2*dt2))

                std[0] = sigma * sqrt((3*(3*n*n - 3*n + 2)) / (n*(n+1)*(n+2)))
                std[1] = (sigma/dt) * sqrt((12*(16*n*n - 30*n + 11)) /
                                           (n*(n*n - 1)*(n*n - 4)))
                std[2] = (sigma/dt2) * sqrt(720 / (n*(n*n-1)*(n*n-4)))

            return error, std

        def __repr__(self):
            return '\n'.join([
                'LeastSquaresFilter object',
                pretty_str('dt', self.dt),
                pretty_str('sigma', self.sigma),
                pretty_str('_order', self._order),
                pretty_str('x', self.x),
                pretty_str('K', self.K)
                ])