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

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

# 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,

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, '
)

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):
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))

for i in range(yt.shape):
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
# (for communication to callback functions):
data = {'r': 3.0, 'd': 4.0, 'Delta': 5.814, 'A': 0.464}
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)

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):
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
)
```