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:
//! updates the predicted state from the measurement
CV_WRAP const Mat& correct( const Mat& measurement );
//! sets predicted state
CV_WRAP void setStatePre( const Mat& state ) { statePre = state; }
//! sets corrected state
CV_WRAP void setStatePost( const Mat& state ) { statePost = state; }
//! sets transition matrix
CV_WRAP void setTransitionMatrix( const Mat& transition ) { transitionMatrix = transition; }
//! sets control matrix
CV_WRAP void setControlMatrix( const Mat& control ) { controlMatrix = control; }
//! sets measurement matrix
CV_WRAP void setMeasurementMatrix( const Mat& measurement ) { measurementMatrix = measurement; }
//! 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)
CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)
CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)
CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)
CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
// temporary matrices
Mat temp1;
......
......@@ -11,21 +11,15 @@
Pressing any key (except ESC) will reset the tracking with a different speed.
Pressing ESC will stop the program.
"""
import urllib2
import cv2
from math import cos, sin, sqrt
import sys
from math import cos, sin
import numpy as np
if __name__ == "__main__":
img_height = 500
img_width = 500
img = np.array((img_height, img_width, 3), np.uint8)
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
......@@ -34,25 +28,17 @@ if __name__ == "__main__":
while True:
state = 0.1 * np.random.randn(2, 1)
transition_matrix = np.array([[1., 1.], [0., 1.]])
kalman.setTransitionMatrix(transition_matrix)
measurement_matrix = 1. * np.ones((1, 2))
kalman.setMeasurementMatrix(measurement_matrix)
process_noise_cov = 1e-5
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))
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))
np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))
state_angle = state[0, 0]
state_pt = calc_point(state_angle)
......@@ -61,21 +47,22 @@ if __name__ == "__main__":
predict_angle = prediction[0, 0]
predict_pt = calc_point(predict_angle)
measurement = measurement_noise_cov * np.random.randn(1, 1)
measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)
# generate measurement
measurement = np.dot(measurement_matrix, state) + 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)
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)
......@@ -87,8 +74,8 @@ if __name__ == "__main__":
kalman.correct(measurement)
process_noise = process_noise_cov * np.random.randn(2, 1)
state = np.dot(transition_matrix, state) + process_noise
process_noise = kalman.processNoiseCov * np.random.randn(2, 1)
state = np.dot(kalman.transitionMatrix, state) + process_noise
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