1#!/usr/bin/env python 2""" 3 Tracking of rotating point. 4 Rotation speed is constant. 5 Both state and measurements vectors are 1D (a point angle), 6 Measurement is the real point angle + gaussian noise. 7 The real and the estimated points are connected with yellow line segment, 8 the real and the measured points are connected with red line segment. 9 (if Kalman filter works correctly, 10 the yellow segment should be shorter than the red one). 11 Pressing any key (except ESC) will reset the tracking with a different speed. 12 Pressing ESC will stop the program. 13""" 14# Python 2/3 compatibility 15import sys 16PY3 = sys.version_info[0] == 3 17 18if PY3: 19 long = int 20 21import numpy as np 22import cv2 as cv 23 24from math import cos, sin, sqrt 25import numpy as np 26 27def main(): 28 img_height = 500 29 img_width = 500 30 kalman = cv.KalmanFilter(2, 1, 0) 31 32 code = long(-1) 33 34 cv.namedWindow("Kalman") 35 36 while True: 37 state = 0.1 * np.random.randn(2, 1) 38 39 kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]]) 40 kalman.measurementMatrix = 1. * np.ones((1, 2)) 41 kalman.processNoiseCov = 1e-5 * np.eye(2) 42 kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1)) 43 kalman.errorCovPost = 1. * np.ones((2, 2)) 44 kalman.statePost = 0.1 * np.random.randn(2, 1) 45 46 while True: 47 def calc_point(angle): 48 return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int), 49 np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int)) 50 51 state_angle = state[0, 0] 52 state_pt = calc_point(state_angle) 53 54 prediction = kalman.predict() 55 predict_angle = prediction[0, 0] 56 predict_pt = calc_point(predict_angle) 57 58 measurement = kalman.measurementNoiseCov * np.random.randn(1, 1) 59 60 # generate measurement 61 measurement = np.dot(kalman.measurementMatrix, state) + measurement 62 63 measurement_angle = measurement[0, 0] 64 measurement_pt = calc_point(measurement_angle) 65 66 # plot points 67 def draw_cross(center, color, d): 68 cv.line(img, 69 (center[0] - d, center[1] - d), (center[0] + d, center[1] + d), 70 color, 1, cv.LINE_AA, 0) 71 cv.line(img, 72 (center[0] + d, center[1] - d), (center[0] - d, center[1] + d), 73 color, 1, cv.LINE_AA, 0) 74 75 img = np.zeros((img_height, img_width, 3), np.uint8) 76 draw_cross(np.int32(state_pt), (255, 255, 255), 3) 77 draw_cross(np.int32(measurement_pt), (0, 0, 255), 3) 78 draw_cross(np.int32(predict_pt), (0, 255, 0), 3) 79 80 cv.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv.LINE_AA, 0) 81 cv.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv.LINE_AA, 0) 82 83 kalman.correct(measurement) 84 85 process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(2, 1) 86 state = np.dot(kalman.transitionMatrix, state) + process_noise 87 88 cv.imshow("Kalman", img) 89 90 code = cv.waitKey(100) 91 if code != -1: 92 break 93 94 if code in [27, ord('q'), ord('Q')]: 95 break 96 97 print('Done') 98 99 100if __name__ == '__main__': 101 print(__doc__) 102 main() 103 cv.destroyAllWindows() 104