From d6306f8ccbcd214dd806a8fed739eff03978ae40 Mon Sep 17 00:00:00 2001 From: Alexander Panov Date: Fri, 20 Aug 2021 13:57:05 +0300 Subject: [PATCH] Merge pull request #20564 from AleksandrPanov:update_kalman_sample Update kalman sample * updated view and comments, fixed dims * updated view and comments, added statePost --- samples/cpp/kalman.cpp | 62 ++++++++++++++----------- samples/python/kalman.py | 97 ++++++++++++++++++++-------------------- 2 files changed, 85 insertions(+), 74 deletions(-) diff --git a/samples/cpp/kalman.cpp b/samples/cpp/kalman.cpp index 501a749124..daf0ba5a71 100644 --- a/samples/cpp/kalman.cpp +++ b/samples/cpp/kalman.cpp @@ -1,6 +1,6 @@ #include "opencv2/video/tracking.hpp" #include "opencv2/highgui.hpp" - +#include "opencv2/core/cvdef.h" #include using namespace cv; @@ -14,15 +14,19 @@ static void help() { printf( "\nExample of c calls to OpenCV's Kalman filter.\n" " Tracking of rotating point.\n" -" Rotation speed is constant.\n" +" Point moves in a circle and is characterized by a 1D state.\n" +" state_k+1 = state_k + speed + process_noise N(0, 1e-5)\n" +" The speed is constant.\n" " Both state and measurements vectors are 1D (a point angle),\n" -" Measurement is the real point angle + gaussian noise.\n" -" The real and the estimated points are connected with yellow line segment,\n" -" the real and the measured points are connected with red line segment.\n" +" Measurement is the real state + gaussian noise N(0, 1e-1).\n" +" The real and the measured points are connected with red line segment,\n" +" the real and the estimated points are connected with yellow line segment,\n" +" the real and the corrected estimated points are connected with green line segment.\n" " (if Kalman filter works correctly,\n" -" the yellow segment should be shorter than the red one).\n" +" the yellow segment should be shorter than the red one and\n" +" the green segment should be shorter than the yellow one)." "\n" -" Pressing any key (except ESC) will reset the tracking with a different speed.\n" +" Pressing any key (except ESC) will reset the tracking.\n" " Pressing ESC will stop the program.\n" ); } @@ -39,7 +43,9 @@ int main(int, char**) for(;;) { - randn( state, Scalar::all(0), Scalar::all(0.1) ); + img = Scalar::all(0); + state.at(0) = 0.0f; + state.at(1) = 2.f * (float)CV_PI / 6; KF.transitionMatrix = (Mat_(2, 2) << 1, 1, 0, 1); setIdentity(KF.measurementMatrix); @@ -60,36 +66,40 @@ int main(int, char**) double predictAngle = prediction.at(0); Point predictPt = calcPoint(center, R, predictAngle); - randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at(0))); - // generate measurement + randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at(0))); measurement += KF.measurementMatrix*state; double measAngle = measurement.at(0); Point measPt = calcPoint(center, R, measAngle); + // correct the state estimates based on measurements + // updates statePost & errorCovPost + KF.correct(measurement); + double improvedAngle = KF.statePost.at(0); + Point improvedPt = calcPoint(center, R, improvedAngle); + // plot points - #define drawCross( center, color, d ) \ - line( img, Point( center.x - d, center.y - d ), \ - Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \ - line( img, Point( center.x + d, center.y - d ), \ - Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 ) - - img = Scalar::all(0); - drawCross( statePt, Scalar(255,255,255), 3 ); - drawCross( measPt, Scalar(0,0,255), 3 ); - drawCross( predictPt, Scalar(0,255,0), 3 ); - line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 ); - line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 ); - - if(theRNG().uniform(0,4) != 0) - KF.correct(measurement); + img = img * 0.2; + drawMarker(img, measPt, Scalar(0, 0, 255), cv::MARKER_SQUARE, 5, 2); + drawMarker(img, predictPt, Scalar(0, 255, 255), cv::MARKER_SQUARE, 5, 2); + drawMarker(img, improvedPt, Scalar(0, 255, 0), cv::MARKER_SQUARE, 5, 2); + drawMarker(img, statePt, Scalar(255, 255, 255), cv::MARKER_STAR, 10, 1); + // forecast one step + Mat test = Mat(KF.transitionMatrix*KF.statePost); + drawMarker(img, calcPoint(center, R, Mat(KF.transitionMatrix*KF.statePost).at(0)), + Scalar(255, 255, 0), cv::MARKER_SQUARE, 12, 1); + + line( img, statePt, measPt, Scalar(0,0,255), 1, LINE_AA, 0 ); + line( img, statePt, predictPt, Scalar(0,255,255), 1, LINE_AA, 0 ); + line( img, statePt, improvedPt, Scalar(0,255,0), 1, LINE_AA, 0 ); + randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at(0, 0)))); state = KF.transitionMatrix*state + processNoise; imshow( "Kalman", img ); - code = (char)waitKey(100); + code = (char)waitKey(1000); if( code > 0 ) break; diff --git a/samples/python/kalman.py b/samples/python/kalman.py index 654e3de3da..cf152a8700 100755 --- a/samples/python/kalman.py +++ b/samples/python/kalman.py @@ -1,14 +1,18 @@ #!/usr/bin/env python """ Tracking of rotating point. - Rotation speed is constant. + Point moves in a circle and is characterized by a 1D state. + state_k+1 = state_k + speed + process_noise N(0, 1e-5) + The 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. + Measurement is the real state + gaussian noise N(0, 1e-1). + The real and the measured points are connected with red line segment, + the real and the estimated points are connected with yellow line segment, + the real and the corrected estimated points are connected with green 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. + the yellow segment should be shorter than the red one and + the green segment should be shorter than the yellow one). + Pressing any key (except ESC) will reset the tracking. Pressing ESC will stop the program. """ # Python 2/3 compatibility @@ -21,8 +25,7 @@ if PY3: import numpy as np import cv2 as cv -from math import cos, sin, sqrt -import numpy as np +from math import cos, sin, sqrt, pi def main(): img_height = 500 @@ -30,64 +33,62 @@ def main(): kalman = cv.KalmanFilter(2, 1, 0) code = long(-1) - - cv.namedWindow("Kalman") - + num_circle_steps = 12 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) + img = np.zeros((img_height, img_width, 3), np.uint8) + state = np.array([[0.0],[(2 * pi) / num_circle_steps]]) # start state + kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]]) # F. input + kalman.measurementMatrix = 1. * np.eye(1, 2) # H. input + kalman.processNoiseCov = 1e-5 * np.eye(2) # Q. input + kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1)) # R. input + kalman.errorCovPost = 1. * np.eye(2, 2) # P._k|k KF state var + kalman.statePost = 0.1 * np.random.randn(2, 1) # x^_k|k KF state var 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)) - + return (np.around(img_width / 2. + img_width / 3.0 * cos(angle), 0).astype(int), + np.around(img_height / 2. - img_width / 3.0 * sin(angle), 1).astype(int)) + img = img * 1e-3 state_angle = state[0, 0] state_pt = calc_point(state_angle) - + # advance Kalman filter to next timestep + # updates statePre, statePost, errorCovPre, errorCovPost + # k-> k+1, x'(k) = A*x(k) + # P'(k) = temp1*At + Q prediction = kalman.predict() - predict_angle = prediction[0, 0] - predict_pt = calc_point(predict_angle) - - measurement = kalman.measurementNoiseCov * np.random.randn(1, 1) + predict_pt = calc_point(prediction[0, 0]) # equivalent to calc_point(kalman.statePre[0,0]) # generate measurement + measurement = kalman.measurementNoiseCov * np.random.randn(1, 1) 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): - cv.line(img, - (center[0] - d, center[1] - d), (center[0] + d, center[1] + d), - color, 1, cv.LINE_AA, 0) - cv.line(img, - (center[0] + d, center[1] - d), (center[0] - d, center[1] + d), - color, 1, cv.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) - - cv.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv.LINE_AA, 0) - cv.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv.LINE_AA, 0) - + # correct the state estimates based on measurements + # updates statePost & errorCovPost kalman.correct(measurement) + improved_pt = calc_point(kalman.statePost[0, 0]) - process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(2, 1) - state = np.dot(kalman.transitionMatrix, state) + process_noise + # plot points + cv.drawMarker(img, measurement_pt, (0, 0, 255), cv.MARKER_SQUARE, 5, 2) + cv.drawMarker(img, predict_pt, (0, 255, 255), cv.MARKER_SQUARE, 5, 2) + cv.drawMarker(img, improved_pt, (0, 255, 0), cv.MARKER_SQUARE, 5, 2) + cv.drawMarker(img, state_pt, (255, 255, 255), cv.MARKER_STAR, 10, 1) + # forecast one step + cv.drawMarker(img, calc_point(np.dot(kalman.transitionMatrix, kalman.statePost)[0, 0]), + (255, 255, 0), cv.MARKER_SQUARE, 12, 1) + + cv.line(img, state_pt, measurement_pt, (0, 0, 255), 1, cv.LINE_AA, 0) # red measurement error + cv.line(img, state_pt, predict_pt, (0, 255, 255), 1, cv.LINE_AA, 0) # yellow pre-meas error + cv.line(img, state_pt, improved_pt, (0, 255, 0), 1, cv.LINE_AA, 0) # green post-meas error + + # update the real process + process_noise = sqrt(kalman.processNoiseCov[0, 0]) * np.random.randn(2, 1) + state = np.dot(kalman.transitionMatrix, state) + process_noise # x_k+1 = F x_k + w_k cv.imshow("Kalman", img) - - code = cv.waitKey(100) + code = cv.waitKey(1000) if code != -1: break