2be805ce29
1. Following condition is True on each iteration becuase -1 % 0xFF is 255 not -1 code = cv2.waitKey(100) % 0x100 if code != -1: break this were resetting point position on each cycle not on key press as intended 2. Previous small bug were masking serious bug with matrix operation on matrices of incorrect size. As the result on 2nd iteration of internal cycle program has crushed. I have fixed it too, matrix operation was taken from examples/cpp/kalman.cpp where it looks like randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0)))); which is something totally different from previous code here. Example behave as it should now, i.e. point moving by circle trajectory as in C++ example.
97 lines
3.2 KiB
Python
Executable File
97 lines
3.2 KiB
Python
Executable File
#!/usr/bin/python
|
|
"""
|
|
Tracking of rotating point.
|
|
Rotation speed is constant.
|
|
Both state and measurements vectors are 1D (a point angle),
|
|
Measurement is the real point angle + gaussian noise.
|
|
The real and the estimated points are connected with yellow line segment,
|
|
the real and the measured points are connected with red line segment.
|
|
(if Kalman filter works correctly,
|
|
the yellow segment should be shorter than the red one).
|
|
Pressing any key (except ESC) will reset the tracking with a different speed.
|
|
Pressing ESC will stop the program.
|
|
"""
|
|
# Python 2/3 compatibility
|
|
import sys
|
|
PY3 = sys.version_info[0] == 3
|
|
|
|
if PY3:
|
|
long = int
|
|
|
|
import cv2
|
|
from math import cos, sin, sqrt
|
|
import numpy as np
|
|
|
|
if __name__ == "__main__":
|
|
|
|
img_height = 500
|
|
img_width = 500
|
|
kalman = cv2.KalmanFilter(2, 1, 0)
|
|
|
|
code = long(-1)
|
|
|
|
cv2.namedWindow("Kalman")
|
|
|
|
while True:
|
|
state = 0.1 * np.random.randn(2, 1)
|
|
|
|
kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
|
|
kalman.measurementMatrix = 1. * np.ones((1, 2))
|
|
kalman.processNoiseCov = 1e-5 * np.eye(2)
|
|
kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))
|
|
kalman.errorCovPost = 1. * np.ones((2, 2))
|
|
kalman.statePost = 0.1 * np.random.randn(2, 1)
|
|
|
|
while True:
|
|
def calc_point(angle):
|
|
return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int),
|
|
np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))
|
|
|
|
state_angle = state[0, 0]
|
|
state_pt = calc_point(state_angle)
|
|
|
|
prediction = kalman.predict()
|
|
predict_angle = prediction[0, 0]
|
|
predict_pt = calc_point(predict_angle)
|
|
|
|
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
|
|
|
|
# generate measurement
|
|
measurement = np.dot(kalman.measurementMatrix, state) + measurement
|
|
|
|
measurement_angle = measurement[0, 0]
|
|
measurement_pt = calc_point(measurement_angle)
|
|
|
|
# plot points
|
|
def draw_cross(center, color, d):
|
|
cv2.line(img,
|
|
(center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
|
|
color, 1, cv2.LINE_AA, 0)
|
|
cv2.line(img,
|
|
(center[0] + d, center[1] - d), (center[0] - d, center[1] + d),
|
|
color, 1, cv2.LINE_AA, 0)
|
|
|
|
img = np.zeros((img_height, img_width, 3), np.uint8)
|
|
draw_cross(np.int32(state_pt), (255, 255, 255), 3)
|
|
draw_cross(np.int32(measurement_pt), (0, 0, 255), 3)
|
|
draw_cross(np.int32(predict_pt), (0, 255, 0), 3)
|
|
|
|
cv2.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv2.LINE_AA, 0)
|
|
cv2.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv2.LINE_AA, 0)
|
|
|
|
kalman.correct(measurement)
|
|
|
|
process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(2, 1)
|
|
state = np.dot(kalman.transitionMatrix, state) + process_noise
|
|
|
|
cv2.imshow("Kalman", img)
|
|
|
|
code = cv2.waitKey(100)
|
|
if code != -1:
|
|
break
|
|
|
|
if code in [27, ord('q'), ord('Q')]:
|
|
break
|
|
|
|
cv2.destroyWindow("Kalman")
|