code: def two_radar():
      # code is not complete - I was using to test RTS smoother. very similar
      # to two_radary.py in book.
  
      import numpy as np
      import matplotlib.pyplot as plt
  
      from numpy import array
      from numpy.linalg import norm
      from numpy.random import randn
      from math import atan2, radians
  
      from filterpy.common import Q_discrete_white_noise
  
      class RadarStation(object):
          def __init__(self, pos, range_std, bearing_std):
              self.pos = asarray(pos)
  
              self.range_std = range_std
              self.bearing_std = bearing_std
  
          def reading_of(self, ac_pos):
              """Returns range and bearing to aircraft as tuple. bearing is in
              radians.
              """
  
              diff = np.subtract(self.pos, ac_pos)
              rng = norm(diff)
              brg = atan2(diff[1], diff[0])
              return rng, brg
  
          def noisy_reading(self, ac_pos):
              rng, brg = self.reading_of(ac_pos)
              rng += randn() * self.range_std
              brg += randn() * self.bearing_std
              return rng, brg
  
      class ACSim(object):
          def __init__(self, pos, vel, vel_std):
              self.pos = asarray(pos, dtype=float)
              self.vel = asarray(vel, dtype=float)
              self.vel_std = vel_std
  
          def update(self):
              vel = self.vel + (randn() * self.vel_std)
              self.pos += vel
  
              return self.pos
  
      dt = 1.0
  
      def hx(x):
          r1, b1 = hx.R1.reading_of((x[0], x[2]))
          r2, b2 = hx.R2.reading_of((x[0], x[2]))
  
          return array([r1, b1, r2, b2])
          pass
  
      def fx(x, dt):
          x_est = x.copy()
          x_est[0] += x[1] * dt
          x_est[2] += x[3] * dt
          return x_est
  
      vx, vy = 0.1, 0.1
  
      f = UnscentedKalmanFilter(
          dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0
      )
      aircraft = ACSim((100, 100), (vx * dt, vy * dt), 0.00000002)
  
      range_std = 0.001  # 1 meter
      bearing_std = 1 / 1000  # 1mrad
  
      R1 = RadarStation((0, 0), range_std, bearing_std)
      R2 = RadarStation((200, 0), range_std, bearing_std)
  
      hx.R1 = R1
      hx.R2 = R2
  
      f.x = array([100, vx, 100, vy])
  
      f.R = np.diag(
          [
              range_std**2,
              bearing_std**2,
              range_std**2,
              bearing_std**2,
          ]
      )
      q = Q_discrete_white_noise(2, var=0.0002, dt=dt)
      # q = np.array([[0,0],[0,0.0002]])
      f.Q[0:2, 0:2] = q
      f.Q[2:4, 2:4] = q
      f.P = np.diag([0.1, 0.01, 0.1, 0.01])
  
      track = []
      zs = []
  
      for i in range(int(300 / dt)):
          pos = aircraft.update()
  
          r1, b1 = R1.noisy_reading(pos)
          r2, b2 = R2.noisy_reading(pos)
  
          z = np.array([r1, b1, r2, b2])
          zs.append(z)
          track.append(pos.copy())
  
      zs = asarray(zs)
  
      xs, Ps, Pxz, pM, pP = f.batch_filter(zs)
      ms, _, _ = f.rts_smoother(xs, Ps)
  
      track = asarray(track)
      time = np.arange(0, len(xs) * dt, dt)
  
      plt.figure()
      plt.subplot(411)
      plt.plot(time, track[:, 0])
      plt.plot(time, xs[:, 0])
      plt.legend(loc=4)
      plt.xlabel("time (sec)")
      plt.ylabel("x position (m)")
      plt.tight_layout()
  
      plt.subplot(412)
      plt.plot(time, track[:, 1])
      plt.plot(time, xs[:, 2])
      plt.legend(loc=4)
      plt.xlabel("time (sec)")
      plt.ylabel("y position (m)")
      plt.tight_layout()
  
      plt.subplot(413)
      plt.plot(time, xs[:, 1])
      plt.plot(time, ms[:, 1])
      plt.legend(loc=4)
      plt.ylim([0, 0.2])
      plt.xlabel("time (sec)")
      plt.ylabel("x velocity (m/s)")
      plt.tight_layout()
  
      plt.subplot(414)
      plt.plot(time, xs[:, 3])
      plt.plot(time, ms[:, 3])
      plt.ylabel("y velocity (m/s)")
      plt.legend(loc=4)
      plt.xlabel("time (sec)")
      plt.tight_layout()
      plt.show()
  
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 18 for calling a global function with example usage: two_radar() and returns: array, x_est, rng,  brg, rng,  brg, self