Implementation of KalmanFilter in Opencv

Two Functions used: gemm , solve
gemm: Performs generalized matrix multiplication.

C++: void gemm(InputArray src1, InputArray src2, double alpha, InputArray src3, double gamma, OutputArray dst, intflags=0 )

The function performs generalized matrix multiplication similar to the gemm functions in BLAS level 3. For example, gemm(src1,src2, alpha, src3, beta, dst, GEMM_1_T + GEMM_3_T) corresponds to

solve: Solves one or more linear systems or least-squares problems.

C++: bool solve(InputArray src1, InputArray src2, OutputArray dst, int flags=DECOMP_LU)

solution (matrix inversion) method. DECOMP_LU Gaussian elimination with optimal pivot element chosen. DECOMP_CHOLESKY Cholesky  factorization; the matrix src1 must be symmetrical and positively defined. DECOMP_EIG eigenvalue decomposition; the matrix src1 must be symmetrical. DECOMP_SVD singular value decomposition (SVD) method; the system can be over-defined and/or the matrix src1 can be singular. DECOMP_QR QR factorization; the system can be over-defined and/or the matrix src1 can be singular. DECOMP_NORMAL while all the previous flags are mutually exclusive, this flag can be used together with any of the previous; it means that the normal equations are solved instead of the original system  .

The function solve solves a linear system or least-squares problem (the latter is possible with SVD or QR methods, or by specifying the flag DECOMP_NORMAL ):

	namespace cv
	{
	KalmanFilter::KalmanFilter() {}
	KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
	{
	init(dynamParams, measureParams, controlParams, type);
	}
	void KalmanFilter::init(int DP, int MP, int CP, int type)
	{
	CV_Assert( DP > 0 && MP > 0 );
	CV_Assert( type == CV_32F || type == CV_64F );
	CP = std::max(CP, 0);
	statePre = Mat::zeros(DP, 1, type);
	statePost = Mat::zeros(DP, 1, type);
	transitionMatrix = Mat::eye(DP, DP, type);
	processNoiseCov = Mat::eye(DP, DP, type);
	measurementMatrix = Mat::zeros(MP, DP, type);
	measurementNoiseCov = Mat::eye(MP, MP, type);
	errorCovPre = Mat::zeros(DP, DP, type);
	errorCovPost = Mat::zeros(DP, DP, type);
	gain = Mat::zeros(DP, MP, type);
	if( CP > 0 )
	controlMatrix = Mat::zeros(DP, CP, type);
	else
	controlMatrix.release();
	temp1.create(DP, DP, type);
	temp2.create(MP, DP, type);
	temp3.create(MP, MP, type);
	temp4.create(MP, DP, type);
	temp5.create(MP, 1, type);
	}
	const Mat& KalmanFilter::predict(const Mat& control)
	{
	// update the state: x'(k) = A*x(k)
	statePre = transitionMatrix*statePost;
	if( control.data )
	// x'(k) = x'(k) + B*u(k)
	statePre += controlMatrix*control;
	// update error covariance matrices: temp1 = A*P(k)
	temp1 = transitionMatrix*errorCovPost;
	// P'(k) = temp1*At + Q
	gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
	return statePre;
	}
	const Mat& KalmanFilter::correct(const Mat& measurement)
	{
	// temp2 = H*P'(k)
	temp2 = measurementMatrix * errorCovPre;
	// temp3 = temp2*Ht + R
	gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
	// temp4 = inv(temp3)*temp2 = Kt(k)
	solve(temp3, temp2, temp4, DECOMP_SVD);
	// K(k)
	gain = temp4.t();
	// temp5 = z(k) - H*x'(k)
	temp5 = measurement - measurementMatrix*statePre;
	// x(k) = x'(k) + K(k)*temp5
	statePost = statePre + gain*temp5;
	// P(k) = P'(k) - K(k)*temp2
	errorCovPost = errorCovPre - gain*temp2;
	return statePost;
	}
	};

view raw gistfile1.cpp hosted with ❤ by GitHub