new_contents: # Inherit
  Leading Edge
  
  #!/usr/bin/python
  
  import urllib2
  import cv2.cv as cv
  from math import cos, sin, sqrt
  import sys
  
  if __name__ == "__main__":
      A = [ [1, 1], [0, 1] ]
  
      img = cv.CreateImage((500, 500), 8, 3)
      kalman = cv.CreateKalman(2, 1, 0)
      state = cv.CreateMat(2, 1, cv.CV_32FC1)  # (phi, delta_phi)
      process_noise = cv.CreateMat(2, 1, cv.CV_32FC1)
      measurement = cv.CreateMat(1, 1, cv.CV_32FC1)
      rng = cv.RNG(-1)
      code = -1L
  
      cv.Zero(measurement)
      cv.NamedWindow("Kalman", 1)
  
      while True:
          cv.RandArr(rng, state, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1))
  
          kalman.transition_matrix[0,0] = 1
          kalman.transition_matrix[0,1] = 1
          kalman.transition_matrix[1,0] = 0
          kalman.transition_matrix[1,1] = 1
  
          cv.SetIdentity(kalman.measurement_matrix, cv.RealScalar(1))
          cv.SetIdentity(kalman.process_noise_cov, cv.RealScalar(1e-5))
          cv.SetIdentity(kalman.measurement_noise_cov, cv.RealScalar(1e-1))
          cv.SetIdentity(kalman.error_cov_post, cv.RealScalar(1))
          cv.RandArr(rng, kalman.state_post, cv.CV_RAND_NORMAL, cv.RealScalar(0), cv.RealScalar(0.1))
  
  
          while True:
              def calc_point(angle):
                  return (cv.Round(img.width/2 + img.width/3*cos(angle)),
                           cv.Round(img.height/2 - img.width/3*sin(angle)))
  
              state_angle = state[0,0]
              state_pt = calc_point(state_angle)
  
              prediction = cv.KalmanPredict(kalman)
              predict_angle = prediction[0, 0]
              predict_pt = calc_point(predict_angle)
  
              cv.RandArr(rng, measurement, cv.CV_RAND_NORMAL, cv.RealScalar(0),
                         cv.RealScalar(sqrt(kalman.measurement_noise_cov[0, 0])))
  
              # generate measurement
              cv.MatMulAdd(kalman.measurement_matrix, state, measurement, measurement)
  
              measurement_angle = measurement[0, 0]
              measurement_pt = calc_point(measurement_angle)
  
              # plot points
              def draw_cross(center, color, d):
                  cv.Line(img, (center[0] - d, center[1] - d),
                               (center[0] + d, center[1] + d), color, 1, cv.CV_AA, 0)
                  cv.Line(img, (center[0] + d, center[1] - d),
                               (center[0] - d, center[1] + d), color, 1, cv.CV_AA, 0)
  
              cv.Zero(img)
              draw_cross(state_pt, cv.CV_RGB(255, 255, 255), 3)
              draw_cross(measurement_pt, cv.CV_RGB(255, 0,0), 3)
              draw_cross(predict_pt, cv.CV_RGB(0, 255, 0), 3)
              cv.Line(img, state_pt, measurement_pt, cv.CV_RGB(255, 0,0), 3, cv. CV_AA, 0)
              cv.Line(img, state_pt, predict_pt, cv.CV_RGB(255, 255, 0), 3, cv. CV_AA, 0)
  
            cv.KalmanCorrect(kalman, measurement)
  
              cv.RandArr(rng, process_noise, cv.CV_RAND_NORMAL, cv.RealScalar(0),
                         cv.RealScalar(sqrt(kalman.process_noise_cov[0, 0])))
              cv.MatMulAdd(kalman.transition_matrix, state, process_noise, state)
  
              cv.ShowImage("Kalman", img)
  
              code = cv.WaitKey(100) % 0x100
              if code != -1:
                  break
  
          if code in [27, ord('q'), ord('Q')]:
              break
  
      cv.DestroyWindow("Kalman")
  
subject: Create Kalman.py