#!/usr/bin/env python3
"``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
)
