# The Unscented Kalman Filter (UKF) for  Nonlinear Estimation Problems

*Cem Özen*, September 2017.


## The Kalman Filter

The Kalman filter is an algorithm that uses the past measurements of the state of a system (also taking into account statistical noise and other inaccuracies) to predict the future state of the system. Its first use was on the Apollo missions to the Moon, but today Kalman filtering is extensively used in a vast array of applications in fields ranging from robotics to econometrics.   

The Kalman filter was originally invented to model linear systems. However, extensions of the method to deal with nonlinear systems have also been developed, such as the extended Kalman filter (EKF) and the unscented Kalman filter (UKF).

In the **unscented Kalman filter (UKF)**, the state distribution is approximated by Gaussian random variables (GRVs)  as in the extended Kalman filter (EKF). However the two methods differ in the way GRVs are propagated through the system dynamics: While the EKF propagates GRVs analytically through a first-order linearization of the non-linear system, the UKF uses a deterministic sampling approach, in which a minimal set of sample points (so-called *sigma points*) that capture the true mean and covariance of the GRV is propagated through the *true* nonlinear system. While the posterior mean and the covariance of the EKF achieves only first-order accuracy (Taylor expansion), which often yields sub-optimal performance, the UKF accuracy is in the 2nd order; thus, the UKF presents a superior alternative to the EKF. Remarkably, this performance advantage does not come at an extra computational cost. 

## Problem Definition
In this notebook, I use the Kalman filter approach (UKF) as a design tool to construct a dynamical system with a desired type of behavior. Qualitatively, (attractive) dynamical systems can exhibit three distinct types of dynamical behavior in the limit $t \rightarrow \infty$: *fixed points*, *oscillations* and *chaos*.
Accordingly a dynamical system with unknown parameters, can be designed (i.e. its parameters can be inferred) so that it displays a desired dynamical behavior. In such a problem, the UKF method is used in the context of *parameter estimation*. 

The parameter estimation (inference) problem is formulated as a state-space model in which a nonlinear mapping,  

$$
\begin{eqnarray*}
y_k & = & g(x_k, \theta_k)
\end{eqnarray*}
$$


with the input $x_k$, the output $y_k$, and the parameters to be inferred, $\theta_k$, is reformulated in the state-space representation: 


$$
\begin{eqnarray*}
\theta_k & = & \theta_{k-1} + \nu_k \\
y_k & = & g(x_k, \theta_k) + u_k.
\end{eqnarray*}
$$

Above $u_k \sim N(0, Q_k)$ represents the measurement noise and $\nu_k \sim N(0, R_k)$ is the artifical process noise which drives the system. 

In the context of the current problem, that is, designing a dynamical system of desired behavior, the nonlinear mapping $g(.)$ will be a nonlinear numerical routine that outputs the dynamical behavior of the system. In the dynamical systems theory, this behavior is encoded in the maximum of the Lyapunov exponents ($\lambda_{max}$) of the system. Hence, in our problem

$$
\begin{eqnarray*}
\theta_k & = & \theta_{k-1} + \nu_k \\
\lambda_{max} & = & \mathfrak{L}(\theta_k, y_0; f) + u_k,
\end{eqnarray*}
$$

where $\mathfrak{L}(.)$ is the nonlinear mapping the system parameters to the target dynamical behavior encoded by the $\lambda_{max}$. Above, $y_0$ is the initial condition for the (n-dimensional) dynamical system, which evolves in time according to

$$
\frac{dy}{dt} = f(y; \theta).
$$

where $f$ is a gradient field. Also, notice that the role of our filter in this context will be  a *smoother*, since at each step of the time series, the observed 'data' remains the same: $(\lambda_{max}, \lambda_{max}, \lambda_{max}, \ldots)$.

**To summarize, given a dynamical system described by a gradient field $f$, its initial conditions $y_0$, and a set of indetermined parameters $\theta$, we seek to infer the values of the parameters to drive the system to produce the desired behavior as encoded by $\lambda_{max}$. **


```python
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import scipy
from scipy import integrate
import sympy
from IPython.display import display, Math, Latex
sympy.init_printing(use_latex='mathjax')
%matplotlib inline
```

### The Dynamical System

Here I apply the UKF technique for the purpose of parameter estimation in the Lorenz model which serves as a good toy model for many applications involving chaos. The Lorenz system is given by the following autonomous dynamical system equations:

$$\begin{align*}
  \dot{x} &= \sigma(y-x), \\
  \dot{y} &= x(\rho-z)-y, \\
  \dot{z} &= xy-\beta z.
\end{align*}$$

In his orginal paper [E. N. Lorenz, J. Atmos. Sci. 20, 130 (1963)], Lorenz used the parameters $\sigma = 10$, $\rho  = 28$, and $\beta = 8/3$ for which the trajectories produce a strange attractor(i.e chaotic dynamics). In our problem, this dynamical behavior (more accurately, the maximal Lyapunov exponent of the Lorenz system with this choice of the parameter values) will be our target. We will initialize the parameter values so that the Lorenz model produces non-chaotic (say, fixed point) dynamics. The unscented Kalman filter will then be utilized to drive the system from this non-chaotic behavior to the chaotic behavior defined by the target value of the maximal Lyapunov exponent; in the process the parameter values will be updated iteratively.  


```python
class LorenzSystem:
    def __init__(self, sigma = 10, rho = 28, beta = 8./3.):
        self.sigma = sigma
        self.rho   = rho
        self.beta  = beta
        
    def dx_dt(self, x, t = None):
        return np.array([ self.sigma * (x[1] - x[0]), 
                         x[0] * (self.rho - x[2]) - x[1], 
                         x[0] * x[1] - self.beta * x[2]])
```

### ODE Solver

In order to integrate the dynamical system equations using given values of initial points, we can use several standard routines available in `scipy.integrate`. However, here I prefer to use an explicit implementation utilizing a 4th order Runge-Kutta step.


```python
class ODESolver:
    def __init__(self, f, dt):
        """ f is function in the form f=f(x,t) """
        self.f = f
        self.dt = dt
    
    def advance(self):
        """Advance solution one time step"""
        raise NotImplementedError
        
    def set_initial_condition(self, u0, t0=0.):
        self.u = []   # u[k] is solution at time t[k]
        self.t = []   # time levels in the solution process
        
        self.u.append(u0)
        self.t.append(t0)
        self.k = 0  # time level counter 
                
    def solve(self, T, terminate=None):
        """ Advance solution from t = t0 to t <= T, steps of dt
        as long as terminate(u, t, k) is False. 
        terminate(u, t, k) is a user-given function returning True or False.
        By default, a terminate function which always returns False is used """
        if terminate is None:
            terminate = lambda u, t, k: False
        self.k = 0
        tnew   = 0
        while tnew <= T and not terminate(self.u, self.t, self.k):
            unew = self.advance()
            self.u.append(unew)
            tnew = self.t[-1] + self.dt    
            self.t.append(tnew)
            self.k += 1
        return np.array(self.u), np.array(self.t)
    
class RungeKutta4(ODESolver):
    def advance(self):
        u, dt, f, k, t = self.u, self.dt, self.f, self.k, self.t[-1]
        dt2 = dt/2.0
        k1  = dt * f(u[k], t)
        k2 =  dt * f(u[k] + 0.5*k1, t + dt2)
        k3 =  dt * f(u[k] + 0.5*k2, t + dt2)
        k4 =  dt * f(u[k] + k3, t + dt)
        unew = u[k] + (1./6.)*(k1 + 2*k2 + 2*k3 + k4)
        return unew
        
```

### Calculation of the Lyapunov Spectra

The concept of Lyapunov exponents was introduced in the dynamical systems theory for the purpose of measuring the sensitivity of the system to initial conditions. In an n-dimensional dynamical system, we can consider an initially orthonormal axes of n vectors in the tangent space at $y_0$. As the dynamical system evolve in time, the volume defined by the initially orthonormal axes get distorted to form an n-dimensional ellipsoid in the tangent space at each successive point on the trajectory.  An algorithm due to Benettin et.al (see Parker and Chua, 1989 in references) computes the average rate of growth (during the time evolution of the system) of the ith principal axis of the ellipsod as $\lambda_i$. These quantities, sorted as $\lambda_1 \ge \lambda_2, \ldots, \lambda_n$ are called the Lyapunov exponents. The sign of the maximum of these exponents, determines the asymptotic dynamical behavior of the dynamical system. In particular, dynamical systems with $\lambda_{max} < 0 $ converge to the same stable fixed point in the phase space. Systems with  $\lambda_{max} = 0 $ converge to an oscillatory solution defined by a limit-cycle and systems with $\lambda_{max} > 0 $ converge to a limit set of solutions defined by a strange (i.e. chaotic) attractor.


In the Benettin algorithm, the Lyapunov spectra are obtained iteratively and directly from the dynamical system equations---unlike various other methods using time series. Since the calculation of Lyapunov exponents is computationally very expensive, in the following I implemented the Benettin algorithm using the `SymPy` library in order to compute the derivatives (hence the Jacobian matrix) in a reliable and fast approach using symbolical computation. 


```python
def sym_to_np(x, t, params, xdot):
    dim       = xdot.shape[0]
    eta       = sympy.Matrix(sympy.symarray('eta', (dim,dim))) # perturbation matrix
    Df        = xdot.jacobian(x)
    etadot    = Df * eta
    z         = x.col_join(eta.reshape(dim*dim,1))
    zdot      = xdot.col_join(etadot.reshape(dim*dim,1)) 
    # lambdification to obtain a NumPy-aware function to compute zdot:
    mat2array = [{'ImmutableMatrix': np.array}, 'numpy']
    zdot_     = sympy.lambdify((z, t, params), zdot, modules=mat2array)
    zdot_np   = lambda z, t, params: zdot_(z, t, params).reshape((dim*dim + dim,)) 
    return zdot_np

def update_state_and_perturbation(zdot_np, params, x, u, tstart=0, tstop=1.0, dt=0.01, integrator='RK4'):
    dim                      = x.shape[0]
    Phi0                     = np.eye(dim)                 # initial value for perturbation matrix Phi
    Phi0_                    = Phi0.reshape((dim*dim,))
    z0                       = np.concatenate((x, Phi0_))  # initial value for combined {x, Phi} 
    if integrator == 'RK4':
        z_solver                 = RungeKutta4(lambda z, t: 
                                                    zdot_np(z, t, params), dt)
        z_solver.set_initial_condition(z0, tstart)
        z, t                     = z_solver.solve(tstop)
    elif integrator == 'ODEINT':
        t                         = np.arange(tstart, tstop, dt) # time measurements
        z                         = integrate.odeint(lambda z, t: zdot_np(z, t, params), z0, t)
    x   = z[-1, 0:dim]
    Phi = z[-1, dim:dim*dim+dim].reshape((dim,dim))
    dx  = np.dot(Phi,u)
    return x, dx

def lyapunov_gram_schimidt(zdot_np, params, x, T=0.2, dt=0.01, Er=1.e-4, Ea=1.e-4, kmax=1000, 
                           integrator='RK4', complete=True, debug=False):
    """ 
    Remarks: 
    1) Make sure the initial state array, x, is already on the attractor for
    a chaotic system. It is best to externally evolve the system until the trajectory
    is settled on the strange attractor, then an arbitrary value of the state can be taken
    as an initial value for this program.
    2) How to choose a suitable value for T: Too small a value would mean loss of accuracy
    due to excessive orthogonalization, too large a value would build up too large numbers hence
    loss of numerical precission. Best values are for systems 
    """
    dim      = x.shape[0]       # dimension of state space
    u        = np.eye(dim)      # initial perturbation matrix, Remark 2, p.79,  Parker and Chua.
    
    if complete is True:        # True for computing all, False for computing maximum Lyapunov exp.
        nums = dim
    else:
        nums = 1
        
    lyaps    = np.zeros((nums,)) # initialize lyapunov exponents
    sum_     = np.zeros((nums,))           
    
    if debug is True:           # True for storing Lyapunov exponent at each iteration
        lyaps_evol = []

    k        = 0
    while True:
        k += 1
        if (k == kmax):
            print("lyapunov_gram_schimidt: no convergence!")
            break
        lyaps_old   = lyaps.copy()
        # dx is the linearized, tangent-space perturbation. We evolve the trajectory x
        # and the perturbation together for a period of T. We output the result to perform a 
        # Gram-Schimidt orthonormalization.
        x, dx       = update_state_and_perturbation(zdot_np, params, x, u, tstart=0, tstop=T, dt=dt, 
                                                    integrator=integrator)
        # perform the orthonormalization and update Lyapunov exponents
        for i in range(nums):
            vi = dx[:, i]
            for j in range(i):
                vi = vi - np.dot(vi, u[:,j]) * u[:,j]
            norm = np.linalg.norm(vi)
            u[:,i] = vi / norm
            sum_[i] += np.log(norm)
            lyaps[i] = sum_[i] / (k * T)
            #print("iter={:d}  LE[{:d}] = {:6.3f}".format(k, i, lyaps[i]))
        if debug is True:
            lyaps_evol.append(np.copy(lyaps))        
        if (np.linalg.norm(lyaps_old - lyaps) < Er * np.linalg.norm(lyaps) + Ea):
            break
            
    if debug is True:
        return np.array(lyaps_evol)
    else:
        return lyaps
```

In the following we will use the Benettin algorithm to compute only the maximal Lyapunov exponent, which solely determines the qualitative behaviour of the system. Before moving on to the UKF method and its application, let's compute the maximal Lyapunov exponent of the Lorenz system for the choice of system parameters $\sigma=10., \rho=28.,$ and $\beta=8./3$:


```python
# Define the Lorenz system, SymPy way:
u         = sympy.Matrix(sympy.symarray('u', 3))             # dynamic variable symbols
t, sigma, rho, beta = sympy.symbols("t, sigma, rho, beta")   # time and system parameter symbols
params    = sympy.Matrix([sigma, rho, beta])                 # parameter vector
lorenz    = sympy.Matrix([sigma * (u[1] - u[0]), u[0] * (rho - u[2]) - u[1], u[0] * u[1] - beta * u[2]])
# Produce the Numpy-aware function (state + perturbation) for the Lorenz system
lorenz_np = sym_to_np(u, t, params, lorenz)
# Now calculate the Lyapunov exponents of the Lorenz System for the chosen parameter set below.
x0        = np.array([ 2, 0.5, 10])   # initial condition for state 
params    = np.array([10., 28., 8./3.])                            # Lorenz system parameters

lyap = lyapunov_gram_schimidt(zdot_np=lorenz_np, params=params, x=x0, T=1.0, dt=0.01, Er=1.e-4,Ea=1.e-4, kmax=10000, 
                             integrator="RK4", complete=False, debug=False)
print("lambda_max = {:.3f}".format(float(lyap)))
```

    lambda_max = 0.883


As a numerical check of the accuracy of the Benettin algorithm, compare the value we obtained above, $\lambda_{max} = 0.883$, to the reported value of $\lambda_{max} = 0.906$ (Sprott, 1997). We can improve the accuracy by using smaller $dt$ or trying different values for $T$ parameter in the Benettin algorithm (see reference Parker and Chua), but we want to be able to run the Benettin algorithm fast enough, since we will call it many times in the filter.

### The Unscented Kalman Filter



The main idea behind the Unscented Kalman Filter (UKF) is to produce several sampling points (the sigma points) around the current state estimate based on its covariance. Once the sigma points are chosen, we propagate these points through the nonlinear mapping to get a more accurate estimation of the mean and the covariance. The application of the filter is therefore an iterative procedure. Each iteration is composed of a prediction step and an update step.

In the "prediction step", we perturb the current parameter estimate by the driving process noise $\nu_k$ to obtain *a priori* estimates of the mean ($\hat{\theta}_k^{pr}$) and the covariance ($P_k^{pr}$) of the parameters, which are conditional on all but the current observation. In the "update step", we use the current observation ($y_k$) to calculate the *a posteriori* estimates of the mean ($\hat{\theta}_k^{po}$) and the covariance ($P_k^{po}$).


*Initialization*

$$\begin{eqnarray*}
\theta_0^{po} & = & E(\theta) \\
P_0^{po} & = & E( (\hat{\theta_0} - \theta) (\hat{\theta_0} - \theta)^T)
\end{eqnarray*}$$

*Prediction*

$$\begin{eqnarray*}
\hat{\theta}_k^{pr} & = & E(\theta | y_{i \leq k-1})  \\
                    & = & \hat{\theta}_{k-1}^{po} \\
P_k^{pr}            & = & P_{k-1}^{po} + R_{k-1}
\end{eqnarray*}$$

*Update*

$$\begin{eqnarray*}
	\hat{\theta}_k^{po} & = & \hat{\theta}_k^{pr} + K_k(y_k - \hat{y}_k)\\
	P_k^{po}            & = & P_k^{pr} - K_k P_{\hat{y}_k} K_k^T
\end{eqnarray*}$$

where 

$$\begin{eqnarray*}
Y_k & = & g(x_k, \Theta_k) \\
\hat{y}_k & = & \sum_{i=0}^{2L} \omega_i^m [Y_k]_i \\
P_{\hat{y}_k} & = & \sum_{i=0}^{2L} \omega_i^c ([Y_k]_i -\hat{y}_k ) ([Y_k]_i  -\hat{y}_k )^T + Q_k \\
P_{\hat{\theta}^{pr}_k \hat{y}_k} & = & \sum_{i=0}^{2L} \omega_i^c ([\Theta_k]_i - \hat{\theta}_k^{pr}) ([Y_k]_i  -\hat{y}_k )^T \\
K_k & = & P_{\hat{\theta}^{pr}_k \hat{y}_k} P_{\hat{y}_k}^{-1}
\end{eqnarray*}$$

The set of sigma points $\Theta_k$ and the corresponding weights are given by

$$\begin{align*}
	[\Theta_k]_0  & =  \hat{\theta}_k^{pr} &     &      &    \omega_0^m & =   \frac{\lambda}{L+\lambda} & i & =0 \\
	[\Theta_k]_i  & =  \hat{\theta}_k^{pr} + \left[\sqrt{ (L+\lambda) P_k^{pr}}\right]_i &   i & =  1, \ldots, L  & \omega_0^c & =  \frac{\lambda}{L + \lambda} + (1-\alpha^2 + \beta)  &  i & =  0 \\
	[\Theta_k]_i  & =  \hat{\theta}_k^{pr} - \left[\sqrt{ (L+\lambda) P_k^{pr}}\right]_i &   i & =  L+1, \ldots, 2L  & \omega_i^c & = \omega_i^m = \frac{1}{2(L + \lambda)} &  i & =  1, \ldots, 2L
\end{align*}$$



```python
class UKF:
    def __init__(self, y_target, func, x0, theta0, P0, alpha, beta, kappa, Q, R):    
        """func must return a numpy array"""
        self.y_target   = y_target
        self.func       = func           # external function y=f(x0, *args). [ *list(theta0) -> *args ] 
        self.N,         = y_target.shape # dimension of the observation space, y=f(x) 
        self.x0         = x0             # initial signal, numpy array of (M,)
        self.theta0     = theta0         # initial mean-values of parameters, numpy array of (L,)
        self.P0         = P0             # initial covariance matrix, numpy array of (L,L)
        self.L,         = theta0.shape   # dimension of parameter space
        self.M,         = x0.shape       # dimension of signal space
        self.alpha      = alpha          # UKF parameter
        self.beta       = beta           # UKF parameter
        self.kappa      = kappa          # UKF parameter
        self.Q          = Q              # measurement noise covariance in the state-space model
        self.R          = R              # artifical process noise covariance in the state-space model

        self.lambda_    = self.alpha**2 * (self.L + self.kappa) - self.L
        self.num_sigmas = 2*self.L + 1   # UKF parameter, number of sigma points  
        Wc = np.full(self.num_sigmas, 1./(2*(self.L + self.lambda_)))
        Wm = Wc.copy()
        Wm[0] = self.lambda_ / (self.L + self.lambda_)
        Wc[0] = Wm[0] + (1. - self.alpha**2 + self.beta)
        self.Wc         = Wc
        self.Wm         = Wm
        # store values
        self.all_theta      = None
        self.all_P          = None
        self.all_y          = None
 
    
    def sigma_points(self, theta, P):
        #returns sigma points for a distribution of parameters (Theta) and
        #for distribution of measurements (Y)
        # calculate the sigma points for the parameters
        Theta = np.zeros((self.num_sigmas, self.L))
        U = scipy.linalg.cholesky((self.L + self.lambda_) * P)
        Theta[0] = theta
        for k in range(self.L):
            Theta[k + 1]          = theta + U[k]
            Theta[self.L + k + 1] = theta - U[k]
        # calculate the sigma-points for the measurements
        Y = np.empty((self.num_sigmas,self.N))
        for i in range(self.num_sigmas):
            Y[i] = self.func(self.x0, *list(Theta[i]))
        return Theta, Y
  
    def unscented_transform(self, Y):
        # use unscented transform to get the mean and covariance for observations, y and Py
        y  = np.dot(self.Wm, Y)
        Py = np.zeros((self.N, self.N))
        for i in range(self.num_sigmas):
            Py += self.Wc[i] * np.outer(Y[i] - y, Y[i] - y)
        return y, Py
                

    def update(self, theta_pr, P_pr):
        #predict values of mean and covariance are updated
        Theta, Y = self.sigma_points(theta_pr, P_pr)
        # mean and covariance of prediction passed through UT
        y, Py = self.unscented_transform(Y)
        Py   += self.Q
        # compute cross variance of the state and the measurements
        Pty = np.zeros((self.L, self.N))
        for i in range(self.num_sigmas):
            Pty += self.Wc[i] * np.outer(Theta[i] - theta_pr, Y[i] - y)
        # compute Kalman gain
        K = np.dot(Pty, scipy.linalg.inv(Py))
        theta = theta_pr +  np.dot(K, self.y_target - y)
        P = P_pr - K.dot(Py).dot(K.T)
        return theta, P
         
    def estimate(self, x0, iter_):
        self.all_theta = np.zeros((iter_, self.L))
        self.all_P     = np.zeros((iter_, self.L, self.L))
        self.all_y     = np.zeros((iter_, self.N))
        theta = self.theta0    # initial value of mean.
        P     = self.P0        # initial value of cov.
        self.all_theta[0,:] = self.theta0
        self.all_P[0, :, :] = P
        for i in range(1, iter_):
            # predict step
            theta_pr = theta
            P_pr     = P + self.R
            # update step
            theta, P = self.update(theta_pr, P_pr)
            self.all_theta[i, :]    = theta
            self.all_P[i, :, :]     = P
            self.all_y[i, :] = self.func(x0, *list(theta))
            if i % 50 == 0 :    # let's print updated values of the parameters at certain intervals
                print("iteration = {:4d}: sigma = {:.2f}, rho = {:.2f}, beta = {:.2f}".format(i, *list(theta) ))


        
```

## Application: Chaotification of the Lorenz System

The Lorenz system that we defined above will serve as a suitable toy model for a parameter estimation application. Let us initialize the Lorenz system in a non-chaotic regime (specifically, a stable fixed point regime). Our goal will be to drive the system to a chaotic regime. In particular, we will conceive a positive real value for the maximal Lyapunov exponent as a target. The UKF then will drive the system to achieve the chaotic dynamics encoded by the maximal Lyapunov exponent by updating the system parameters. 

### A Technical Side Note: Constraining the Parameters in the UKF

In the formulation of the UKF approach, there are no constraints for any of the system parameters. Therefore, in general, each parameter can take any value in $(-\infty, +\infty)$. However, like many other physical systems, the Lorenz model parameters need to be constrained to positive real numbers. Although this can be achieved through modifying the UKF equations, I opt to improvise a quick-and-dirty approach to achieve the same end. The idea here is to introduce an extra dimension in the observable function (i.e. the nonlinear mapping $g(.)$ in the general formulation) for every parameter to be constrained. These extra "observables" can then be used to assign penalties (such as large positive values) any time the UKF updates the parameters to values lying outside the allowed window, which may be bounded in either or both sides by a finite value. As long as the parameter values remain within the boundaries, no penalty would be given. 


```python
def penalty(x, lb=0, ub=1):
    const = 100.
    if x < lb :
        return const * np.exp(abs(x-lb)**2)
    elif x > ub:
        return const * np.exp(abs(x-ub)**2)
    else: 
        return 0.


def g(x, sigma, rho, beta):
    """ 
    non-linear observable function 
    x    : (3, ) numpy array holding state vector coordinates
    theta: (3, ) numpy array holding values of sigma, rho and beta
    """
    tstart, tstop, dt  = 0., 10., 0.01   
    lorenz_model = LorenzSystem(sigma, rho, beta)   
    lorenz_model_derivatives = lorenz_model.dx_dt
    lorenz_solver = RungeKutta4(lorenz_model_derivatives, dt)
    lorenz_solver.set_initial_condition(x, tstart)
    x, tt  = lorenz_solver.solve(tstop)
    x0     = x[-1,:]                        # The end point of the trajectory is the new initial point
                                            # for all the computations below
    
    # OBSERVABLES TO EXPORT
    # maximal Lyapunov exponent
    params = np.array([sigma, rho, beta])
    lyap = lyapunov_gram_schimidt(zdot_np=lorenz_np, params=params, x=x0, T=1.0, dt=0.01, Er=1.e-4,Ea=1.e-4, kmax=10000, 
                             integrator="RK4", complete=False, debug=False) 
    
    s_penalty = penalty(sigma, lb=0.5, ub=30)
    r_penalty = penalty(rho,   lb=0.5, ub=30)
    b_penalty = penalty(beta,  lb=0.5, ub=30)

    penalties = np.array([s_penalty, r_penalty, b_penalty])
    return np.concatenate((lyap, penalties))
```

Note that we modified the mapping $g(.)$, which normally would calculate the maximal Lyapunov exponent as $\lambda_{max} =  \mathfrak{L}(\theta_k, y_0; f)$, to return a NumPy array of shape (4,) because we include three penalty values. Above $x$ represents the initial condition, $y_0$, for the Lorenz model's 3-dimensional phase space.  In our problem, $x$ will always remain a constant, so every time we update the parameters to obtain a new Lorenz model, the time evolution will always start with this initial condition.




```python
# initialize state vector in Lorenz model  
x0 = np.array([2.0, 0.5, 1.0])             # arbitrarily chosen

# initialize parameters  (sigma, rho, beta):
theta0 = np.array([10.0, 10.0, 10.0])       # initial mean values of the parameters
P0 = 0.01*np.diag([1, 1, 1])                # initial state covariance

# initialize measurement and noise covariances
Q = 0.01*np.diag([1, 1, 1, 1])              # measurement noise covariance
R = 0.01*np.diag([1, 1, 1])                 # process noise covariance

# initialize UKF parameters (see reference Labbe for explanations)
alpha = 0.8
beta = 2.
kappa = 0.

no_iterations=300                           # number of iterations 

# set the target dynamics
y_target = np.array([0.883, 0., 0., 0.])    # target value of maximal Lyapunov exponent (calculated above)

# create an instance of the filter for our problem
ukf = UKF(y_target, g, x0, theta0, P0, alpha, beta, kappa, Q, R)   
```

### Parameter Estimation (Inference) Using the UKF

Now we are ready to run the filter to infer the Lorenz model's parameter values that would produce the chaotic dynamics encoded by the maximal Layapunov exponent value of 0.883.


```python
ukf.estimate(x0, no_iterations)
iterations = np.arange(no_iterations)
```

    iteration =   50: sigma = 8.32, rho = 20.62, beta = 1.59
    iteration =  100: sigma = 10.61, rho = 25.90, beta = 2.44
    iteration =  150: sigma = 10.79, rho = 27.21, beta = 2.73
    iteration =  200: sigma = 10.73, rho = 27.73, beta = 2.79
    iteration =  250: sigma = 10.69, rho = 27.89, beta = 2.52


Let's print out initial and final values of the relevant quantities:


```python
print("Maximal Lyapunov exponent: target value vs achieved value")
print("y_initial = {:.3f}".format(g(x0, *list(theta0))[0]))
print("y_target = {:.3f}".format(y_target[0]))
print("y last   = {:.3f}\n".format( ukf.all_y[-1,0]))

print("Inferred parameters: initial and achieved values (mean)")
print("sigma = {:.2f}, rho = {:.2f}, beta = {:.2f}".format(*list(ukf.theta0) ))
print("sigma = {:.2f}, rho = {:.2f}, beta = {:.2f}\n".format(*list(ukf.all_theta[-1,:]) ))

print("Inferred parameters: initial and achieved values (covariance)")
print("P init=", P0)
print("P last=", ukf.all_P[-1,:,:])
```

    Maximal Lyapunov exponent: target value vs achieved value
    y_initial = -2.719
    y_target = 0.883
    y last   = 0.916
    
    Inferred parameters: initial and achieved values (mean)
    sigma = 10.00, rho = 10.00, beta = 10.00
    sigma = 10.71, rho = 27.98, beta = 2.59
    
    Inferred parameters: initial and achieved values (covariance)
    P init= [[ 0.01  0.    0.  ]
     [ 0.    0.01  0.  ]
     [ 0.    0.    0.01]]
    P last= [[ 0.92755344  0.06007742 -0.12421053]
     [ 0.06007742  0.63554907 -0.11750524]
     [-0.12421053 -0.11750524  0.15363307]]


In order to be able to plot how the UKF drives the system to produce the desired output, and infer the values of the system parameters, we need to integrate the Lorenz model equations for the parameter values we started with, and also for the parameter values we obtained at the end.


```python
# integration of the Lorenz model (using the parameter values we started with)
tstart, tstop, dt  = 0., 10., 0.01  
x0 = np.array([2.0, 2.5, 5.0])        # initial values for Lorenz model state variables (x,y,z)--we keep this constant 
params = list(ukf.all_theta[0,:])     # initial values for sigma, rho, beta 
lorenz_model = LorenzSystem(*params)  
lorenz_model_derivatives = lorenz_model.dx_dt
lorenz_solver = RungeKutta4(lorenz_model_derivatives, dt)
lorenz_solver.set_initial_condition(x0, tstart)
x_ti, t  = lorenz_solver.solve(tstop)

# integration of the Lorenz model (using the parameter values we started with)
# since we do not change x0, below we use the same x0 as above.
params = list(ukf.all_theta[-1,:])     # final values for sigma, rho, beta 
lorenz_model = LorenzSystem(*params)  
lorenz_model_derivatives = lorenz_model.dx_dt
lorenz_solver = RungeKutta4(lorenz_model_derivatives, dt)
lorenz_solver.set_initial_condition(x0, tstart)
x_tj, t  = lorenz_solver.solve(tstop)
```


```python
fig, ax0 = plt.subplots()
fig.set_size_inches(13,5)

ax0.plot(iterations[1:], ukf.all_y[1:,0], lw=3)
#ax0.annotate('', fontsize = 12, xy = (3, -2), xycoords = 'data', \
#            xytext=(40, -1.5), arrowprops=dict(arrowstyle="->", connectionstyle="arc3,rad=0.1"))
#ax0.annotate('fixed point dynamics', xy = (40, -0.6), xycoords = 'data', fontsize = 12)



ax0.axhline(y=0, c="k")
ax0.axhline(y=y_target[0], c="r")
ax0.set_ylim(ymax=3)
ax0.set_xlabel("iterations", fontsize = 20)
ax0.set_ylabel(r"$\lambda_{max}$", fontsize = 20)


            

ax1 = fig.add_axes([0.20, 0.15, 0.08, 0.25], projection='3d')  # x,y, width, height (all relative to 1)
ax1.axis("on")
ax1.xaxis.set_major_locator(plt.NullLocator())
ax1.yaxis.set_major_locator(plt.NullLocator())
ax1.zaxis.set_major_locator(plt.NullLocator())
ax1.plot(x_ti[:,0], x_ti[:,1], x_ti[:,2], 'r-', lw=2.0)
ax1.view_init(30, 20)  # altitude and azimuth in degrees


ax2 = fig.add_axes([0.80, 0.62, 0.08, 0.25], projection='3d')  # x,y, width, height (all relative to 1)
ax2.axis("on")
ax2.xaxis.set_major_locator(plt.NullLocator())
ax2.yaxis.set_major_locator(plt.NullLocator())
ax2.zaxis.set_major_locator(plt.NullLocator())
ax2.plot(x_tj[:,0], x_tj[:,1], x_tj[:,2], 'r-', lw=0.5)
ax2.view_init(30, 120)  # altitude and azimuth in degrees

ax0.annotate('fixed point dynamics', xy = (60, -1.5), xycoords = 'data', fontsize = 12)
ax0.annotate('chaotic dynamics', xy = (220, 1.8), xycoords = 'data', fontsize = 12)
ax0.annotate(r'$\lambda_{max}^{target}$', xy = (1.02, 0.6), xycoords = 'axes fraction', fontsize = 20, color = "r")
```

The plot above shows how the UKF drives the system from an initial configuration in the fixed-point regime to the chaotic regime (the boundary between these two dynamical regimes is the $\lambda_{max}=0$ line, on which the dynamical behavior is limit cycles). Parameters of the Lorenz model kept getting updated in the chaotic regime until the $\lambda_{max}$ of the model reached $\lambda_{max}^{target}$.


```python
fig, ax = plt.subplots(1,3)
fig.subplots_adjust(wspace=0.5)
fig.set_size_inches(15, 3)

ax[0].set_xlabel("iterations", fontsize = 20)
ax[0].set_ylabel(r"$\sigma$", fontsize = 20)
ax[0].plot(iterations, ukf.all_theta[:,0])

ax[1].set_xlabel("iterations", fontsize = 20)
ax[1].set_ylabel(r"$\rho$", fontsize = 20)
ax[1].plot(iterations, ukf.all_theta[:,1])

ax[2].set_xlabel("iterations", fontsize = 20)
ax[2].set_ylabel(r"$\beta$", fontsize = 20)
ax[2].plot(iterations, ukf.all_theta[:,2])
```

Plots above show the inferred values of the parameters $\sigma$, $\rho$, and $\beta$ at successive UKF iterations.

## References:

1) Labbe, R. R. (2018). *Kalman and Bayesian Filters in Python*. (available online) <br>
2) Parker, T.S. and Chua, L.O. (1989). *Practical Numerical Algorithms for Chaotic Systems*. New York, USA: Springer-Verlag. <br>
3) Silk et.al, *Designing attractive models via automated identification of chaotic and oscillatory dynamical regimes*, Nature Communications, (2011) 2:489. <br>
4) Sprott, J.C. (1997). *Lyapunov Exponent and Dimension of the Lorenz Attractor*, http://sprott.physics.wisc.edu/chaos/lorenzle.htm

e9b30d31f2994fb7fa8969dbf67f0e900222af9d
ipynb
Jupyter Notebook
kalman.ipynb
cemozen/kalman_filtering_for_nonlinear_estimation
c8bdebd18d7255fd86566564f6999f76b3945eec
MIT
2020-12-23T20:10:19.000Z
2020-12-23T20:10:19.000Z
kalman.ipynb
cemozen/kalman_filtering_for_nonlinear_estimation
c8bdebd18d7255fd86566564f6999f76b3945eec
MIT
kalman.ipynb
cemozen/kalman_filtering_for_nonlinear_estimation
c8bdebd18d7255fd86566564f6999f76b3945eec
MIT
2019-12-09T01:26:08.000Z
2019-12-09T01:26:08.000Z
Qwen/Qwen-72B
1. YES
2. YES