Commit dc49e115 authored by Juan Carlos Niebles's avatar Juan Carlos Niebles

removed setter methods, replaced by CV_PROP_RW macro

parent 1162f0ed
...@@ -129,33 +129,16 @@ public: ...@@ -129,33 +129,16 @@ public:
//! updates the predicted state from the measurement //! updates the predicted state from the measurement
CV_WRAP const Mat& correct( const Mat& measurement ); CV_WRAP const Mat& correct( const Mat& measurement );
//! sets predicted state CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
CV_WRAP void setStatePre( const Mat& state ) { statePre = state; } CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
//! sets corrected state CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)
CV_WRAP void setStatePost( const Mat& state ) { statePost = state; } CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
//! sets transition matrix CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)
CV_WRAP void setTransitionMatrix( const Mat& transition ) { transitionMatrix = transition; } CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)
//! sets control matrix CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
CV_WRAP void setControlMatrix( const Mat& control ) { controlMatrix = control; } CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
//! sets measurement matrix CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
CV_WRAP void setMeasurementMatrix( const Mat& measurement ) { measurementMatrix = measurement; } CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
//! sets process noise covariance matrix
CV_WRAP void setProcessNoiseCov( const Mat& noise ) { processNoiseCov = noise; }
//! sets measurement noise covariance matrix
CV_WRAP void setMeasurementNoiseCov( const Mat& noise ) { measurementNoiseCov = noise; }
//! sets posteriori error covariance
CV_WRAP void setErrorCovPost( const Mat& error ) { errorCovPost = error; }
Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Mat transitionMatrix; //!< state transition matrix (A)
Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
Mat measurementMatrix; //!< measurement matrix (H)
Mat processNoiseCov; //!< process noise covariance matrix (Q)
Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
// temporary matrices // temporary matrices
Mat temp1; Mat temp1;
......
...@@ -11,21 +11,15 @@ ...@@ -11,21 +11,15 @@
Pressing any key (except ESC) will reset the tracking with a different speed. Pressing any key (except ESC) will reset the tracking with a different speed.
Pressing ESC will stop the program. Pressing ESC will stop the program.
""" """
import urllib2
import cv2 import cv2
from math import cos, sin, sqrt from math import cos, sin
import sys
import numpy as np import numpy as np
if __name__ == "__main__": if __name__ == "__main__":
img_height = 500 img_height = 500
img_width = 500 img_width = 500
img = np.array((img_height, img_width, 3), np.uint8)
kalman = cv2.KalmanFilter(2, 1, 0) kalman = cv2.KalmanFilter(2, 1, 0)
state = np.zeros((2, 1)) # (phi, delta_phi)
process_noise = np.zeros((2, 1))
measurement = np.zeros((1, 1))
code = -1L code = -1L
...@@ -34,25 +28,17 @@ if __name__ == "__main__": ...@@ -34,25 +28,17 @@ if __name__ == "__main__":
while True: while True:
state = 0.1 * np.random.randn(2, 1) state = 0.1 * np.random.randn(2, 1)
transition_matrix = np.array([[1., 1.], [0., 1.]]) kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])
kalman.setTransitionMatrix(transition_matrix) kalman.measurementMatrix = 1. * np.ones((1, 2))
measurement_matrix = 1. * np.ones((1, 2)) kalman.processNoiseCov = 1e-5 * np.eye(2)
kalman.setMeasurementMatrix(measurement_matrix) kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))
kalman.errorCovPost = 1. * np.ones((2, 2))
process_noise_cov = 1e-5 kalman.statePost = 0.1 * np.random.randn(2, 1)
kalman.setProcessNoiseCov(process_noise_cov * np.eye(2))
measurement_noise_cov = 1e-1
kalman.setMeasurementNoiseCov(measurement_noise_cov * np.ones((1, 1)))
kalman.setErrorCovPost(1. * np.ones((2, 2)))
kalman.setStatePost(0.1 * np.random.randn(2, 1))
while True: while True:
def calc_point(angle): def calc_point(angle):
return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int), 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)) np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))
state_angle = state[0, 0] state_angle = state[0, 0]
state_pt = calc_point(state_angle) state_pt = calc_point(state_angle)
...@@ -61,21 +47,22 @@ if __name__ == "__main__": ...@@ -61,21 +47,22 @@ if __name__ == "__main__":
predict_angle = prediction[0, 0] predict_angle = prediction[0, 0]
predict_pt = calc_point(predict_angle) predict_pt = calc_point(predict_angle)
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
measurement = measurement_noise_cov * np.random.randn(1, 1)
# generate measurement # generate measurement
measurement = np.dot(measurement_matrix, state) + measurement measurement = np.dot(kalman.measurementMatrix, state) + measurement
measurement_angle = measurement[0, 0] measurement_angle = measurement[0, 0]
measurement_pt = calc_point(measurement_angle) measurement_pt = calc_point(measurement_angle)
# plot points # plot points
def draw_cross(center, color, d): def draw_cross(center, color, d):
cv2.line(img, (center[0] - d, center[1] - d), cv2.line(img,
(center[0] + d, center[1] + d), color, 1, cv2.LINE_AA, 0) (center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
cv2.line(img, (center[0] + d, center[1] - d), color, 1, cv2.LINE_AA, 0)
(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) img = np.zeros((img_height, img_width, 3), np.uint8)
draw_cross(np.int32(state_pt), (255, 255, 255), 3) draw_cross(np.int32(state_pt), (255, 255, 255), 3)
...@@ -87,8 +74,8 @@ if __name__ == "__main__": ...@@ -87,8 +74,8 @@ if __name__ == "__main__":
kalman.correct(measurement) kalman.correct(measurement)
process_noise = process_noise_cov * np.random.randn(2, 1) process_noise = kalman.processNoiseCov * np.random.randn(2, 1)
state = np.dot(transition_matrix, state) + process_noise state = np.dot(kalman.transitionMatrix, state) + process_noise
cv2.imshow("Kalman", img) cv2.imshow("Kalman", img)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment