Source code for naginterfaces.​library.​examples.​tsa.​kalman_unscented_state_ex

#!/usr/bin/env python
"``naginterface.library.tsa.kalman_unscented_state`` Python Example."

# NAG Copyright 2017-2019.

# pylint: disable=invalid-name,too-many-locals

from math import cos, pi, sin

import numpy as np

from naginterfaces.library import tsa

[docs]def main(): """ Example for :func:`naginterfaces.library.tsa.kalman_unscented_state`. Combined time and measurement update, one iteration of the Unscented Kalman Filter for a nonlinear state space model, with additive noise. Demonstrates passing user-defined communication data to callback functions. >>> main() naginterfaces.library.tsa.kalman_unscented_state Python Example Results. Applying the UKF to a nonlinear state space model, with additive noise. At time point 0, state estimate is: ( 0.664, -0.092, 0.104) At time point 1, state estimate is: ( 1.598, 0.081, 0.314) At time point 2, state estimate is: ( 2.128, 0.213, 0.378) At time point 3, state estimate is: ( 3.134, 0.674, 0.660) At time point 4, state estimate is: ( 3.809, 1.181, 0.906) At time point 5, state estimate is: ( 4.730, 2.000, 1.298) At time point 6, state estimate is: ( 4.429, 2.474, 1.762) At time point 7, state estimate is: ( 4.357, 3.246, 2.162) At time point 8, state estimate is: ( 3.907, 3.852, 2.246) At time point 9, state estimate is: ( 3.360, 4.398, 2.504) At time point 10, state estimate is: ( 2.552, 4.741, 2.750) At time point 11, state estimate is: ( 2.191, 5.193, 3.281) At time point 12, state estimate is: ( 1.309, 5.018, 3.610) At time point 13, state estimate is: ( 1.071, 4.894, 4.031) At time point 14, state estimate is: ( 0.618, 4.322, 4.124) Estimate of the Cholesky factorization of the state covariance matrix at the last time point: [ 1.92e-01 -3.82e-01, 2.22e-02 1.58e-06, 2.23e-07, 9.95e-03 ] """ print( 'naginterfaces.library.tsa.kalman_unscented_state ' 'Python Example Results.' ) print( 'Applying the UKF to a nonlinear state space model, ' 'with additive noise.' ) def cb_f(xt, data): "The (three-variable) state function." t1 = 0.5*data['r']*(data['phi_rt'] + data['phi_lt']) t3 = (data['r']/data['d'])*(data['phi_rt'] - data['phi_lt']) fxt = np.empty(xt.shape) for i in range(xt.shape[1]): fxt[:, i] = xt[:, i] + [ cos(xt[2, i])*t1, sin(xt[2, i])*t1, t3 ] return fxt def cb_h(my, yt, data): "The (three-variable, two-observation) measurement function." hyt = np.empty((my, yt.shape[1])) for i in range(yt.shape[1]): hyt[:, i] = [ ( data['Delta'] - yt[0, i]*cos(data['A']) - yt[1, i]*sin(data['A']) ), yt[2, i] - data['A'] ] # Make sure that the theta is in the same range as the observed # data, which in this case is [0, 2*pi) if hyt[1, i] < 0: hyt[1, i] = hyt[1, i] + 2*pi return hyt # The initial state vector: mx = 3 x = np.zeros(mx) # The Cholesky factorization of the covariance matrix for the # process noise: lx = np.diag([0.1]*mx) # The Cholesky factorization of the initial state covariance matrix: st = np.diag([0.1]*mx) # Number of time points to run the system for: ntime = 15 # Additional time-independent problem parameters # (for communication to callback functions): data = {'r': 3.0, 'd': 4.0, 'Delta': 5.814, 'A': 0.464} # Additional time-dependent problem parameters: phi_rt = [0.4]*ntime phi_lt = [0.1]*ntime # Observations: y_obs = np.array([ [5.262, 5.923], [4.347, 5.783], [3.818, 6.181], [2.706, 0.085], [1.878, 0.442], [0.684, 0.836], [0.752, 1.300], [0.464, 1.700], [0.597, 1.781], [0.842, 2.040], [1.412, 2.286], [1.527, 2.820], [2.399, 3.147], [2.661, 3.569], [3.327, 3.659], ]) # The Cholesky factorization of the covariance matrix for the # observation noise: ly = np.diag([0.01]*y_obs.shape[1]) for t in range(ntime): data['phi_rt'] = phi_rt[t] data['phi_lt'] = phi_lt[t] y = y_obs[t] x, st = tsa.kalman_unscented_state( y, lx, ly, cb_f, cb_h, x, st, data ) print('At time point {:d}, state estimate is:'.format(t)) print('(' + ', '.join(['{:10.3f}']*len(x)).format(*x) + ')') print('Estimate of the Cholesky factorization of the state') print('covariance matrix at the last time point:') print('[') for i in range(st.shape[0]): print(' ' + ', '.join(['{:10.2e}']*(i+1)).format(*st[i, :(i+1)])) print(']')
if __name__ == '__main__': import doctest import sys sys.exit( doctest.testmod( None, verbose=True, report=False, optionflags=doctest.REPORT_NDIFF, ).failed )