code: def test_circle():
      from filterpy.kalman import KalmanFilter
      from math import radians
  
      def hx(x):
          radius = x[0]
          angle = x[1]
          x = cos(radians(angle)) * radius
          y = sin(radians(angle)) * radius
          return np.array([x, y])
  
      def fx(x, dt):
          return np.array([x[0], x[1] + x[2], x[2]])
  
      std_noise = 0.1
  
      sp = JulierSigmaPoints(n=3, kappa=0.0)
      f = UKF(dim_x=3, dim_z=2, dt=0.01, hx=hx, fx=fx, points=sp)
      f.x = np.array([50.0, 90.0, 0])
      f.P *= 100
      f.R = np.eye(2) * (std_noise**2)
      f.Q = np.eye(3) * 0.001
      f.Q[0, 0] = 0
      f.Q[2, 2] = 0
  
      kf = KalmanFilter(dim_x=6, dim_z=2)
      kf.x = np.array([50.0, 0.0, 0, 0, 0.0, 0.0])
  
      F = np.array(
          [
              [1.0, 1.0, 0.5, 0.0, 0.0, 0.0],
              [0.0, 1.0, 1.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
              [0.0, 0.0, 0.0, 1.0, 1.0, 0.5],
              [0.0, 0.0, 0.0, 0.0, 1.0, 1.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
          ]
      )
  
      kf.F = F
      kf.P *= 100
      kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
  
      kf.R = f.R
      kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1.0, 0.00001)
      kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1.0, 0.00001)
  
      measurements = []
      results = []
  
      zs = []
      kfxs = []
      for t in range(0, 12000):
          a = t / 30 + 90
          x = cos(radians(a)) * 50.0 + randn() * std_noise
          y = sin(radians(a)) * 50.0 + randn() * std_noise
          # create measurement = t plus white noise
          z = np.array([x, y])
          zs.append(z)
  
          f.predict()
          f.update(z)
  
          kf.predict()
          kf.update(z)
  
          # save data
          results.append(hx(f.x))
          kfxs.append(kf.x)
          # print(f.x)
  
      results = np.asarray(results)
      zs = np.asarray(zs)
      kfxs = np.asarray(kfxs)
  
      print(results)
      if DO_PLOT:
          plt.plot(zs[:, 0], zs[:, 1], c="r", label="z")
          plt.plot(results[:, 0], results[:, 1], c="k", label="UKF")
          plt.plot(kfxs[:, 0], kfxs[:, 3], c="g", label="KF")
          plt.legend(loc="best")
          plt.axis("equal")
  
imports: from __future__ import absolute_import, division, print_function, unicode_literals
  import matplotlib.pyplot
  import numpy.random
  from numpy.random import randn
  from numpy import asarray
  import numpy
  from filterpy.kalman import UnscentedKalmanFilter
  from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints, JulierSigmaPoints, SimplexSigmaPoints
  from filterpy.common import Q_discrete_white_noise
  import filterpy.stats
  from math import cos, sin
  from filterpy.kalman import UnscentedKalmanFilter2
label_desc: Use image node_id 16 for calling a global function with example usage: test_circle() and returns: np, np