code: def test_imm():
      """This test is drawn from Crassidis [1], example 4.6.
  
      ** References**
  
      [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press,
      Second edition.
      """
  
      r = 100.0
      dt = 1.0
      phi_sim = np.array(
          [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]
      )
  
      gam = np.array(
          [[dt**2 / 2, 0], [dt, 0], [0, dt**2 / 2], [0, dt]]
      )
  
      x = np.array([[2000, 0, 10000, -15.0]]).T
  
      simxs = []
      N = 600
      for i in range(N):
          x = np.dot(phi_sim, x)
          if i >= 400:
              x += np.dot(gam, np.array([[0.075, 0.075]]).T)
          simxs.append(x)
      simxs = np.array(simxs)
  
      zs = np.zeros((N, 2))
      for i in range(len(zs)):
          zs[i, 0] = simxs[i, 0] + randn() * r
          zs[i, 1] = simxs[i, 2] + randn() * r
  
      """
      try:
          # data to test against crassidis' IMM matlab code
          zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1]
          zs = zs_tmp
      except:
          pass
      """
      ca = KalmanFilter(6, 2)
      cano = KalmanFilter(6, 2)
      dt2 = (dt**2) / 2
      ca.F = np.array(
          [
              [1, dt, dt2, 0, 0, 0],
              [0, 1, dt, 0, 0, 0],
              [0, 0, 1, 0, 0, 0],
              [0, 0, 0, 1, dt, dt2],
              [0, 0, 0, 0, 1, dt],
              [0, 0, 0, 0, 0, 1],
          ]
      )
      cano.F = ca.F.copy()
  
      ca.x = np.array([[2000.0, 0, 0, 10000, -15, 0]]).T
      cano.x = ca.x.copy()
  
      ca.P *= 1.0e-12
      cano.P *= 1.0e-12
      ca.R *= r**2
      cano.R *= r**2
      cano.Q *= 0
      q = (
          np.array(
              [
                  [0.05, 0.125, 1.0 / 6],
                  [0.125, 1 / 3, 0.5],
                  [1.0 / 6, 0.5, 1.0],
              ]
          )
          * 1.0e-3
      )
  
      ca.Q[0:3, 0:3] = q
      ca.Q[3:6, 3:6] = q
  
      ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
      cano.H = ca.H.copy()
  
      filters = [ca, cano]
  
      trans = np.array([[0.97, 0.03], [0.03, 0.97]])
  
      bank = IMMEstimator(filters, (0.5, 0.5), trans)
  
      # ensure __repr__ doesn't have problems
      str(bank)
  
      s = Saver(bank)
      ca_s = Saver(ca)
      cano_s = Saver(cano)
      for i, z in enumerate(zs):
          z = np.array([z]).T
          bank.update(z)
          bank.predict()
  
          s.save()
          ca_s.save()
          cano_s.save()
  
      if DO_PLOT:
          s.to_array()
          ca_s.to_array()
          cano_s.to_array()
  
          plt.figure()
  
          plt.subplot(121)
          plt.plot(s.x[:, 0], s.x[:, 3], "k")
          # plt.plot(cvxs[:, 0], caxs[:, 3])
          # plt.plot(simxs[:, 0], simxs[:, 2], 'g')
          plt.scatter(zs[:, 0], zs[:, 1], marker="+", alpha=0.2)
  
          plt.subplot(122)
          plt.plot(s.mu[:, 0])
          plt.plot(s.mu[:, 1])
          plt.ylim(0, 1)
          plt.title("probability ratio p(cv)/p(ca)")
  
          """plt.figure()
          plt.plot(cvxs, label='CV')
          plt.plot(caxs, label='CA')
          plt.plot(xs[:, 0], label='GT')
          plt.legend()
  
          plt.figure()
          plt.plot(xs)
          plt.plot(xs[:, 0])"""
  
imports: from __future__ import absolute_import, division, print_function, unicode_literals
  from math import sin, cos, radians
  import numpy.random
  import numpy
  from numpy import array
  from numpy.random import randn
  import matplotlib.pyplot
  from filterpy.kalman import IMMEstimator, KalmanFilter
  from filterpy.common import Q_discrete_white_noise, Saver
label_desc: Use image node_id 5 for calling a global function with example usage: test_imm() without return types