+++ /dev/null
-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-// By downloading, copying, installing or using the software you agree to this license.
-// If you do not agree to this license, do not download, install,
-// copy or use the software.
-//
-//
-// Intel License Agreement
-// For Open Source Computer Vision Library
-//
-// Copyright (C) 2000, Intel Corporation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-// * Redistribution's of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-//
-// * Redistribution's in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-//
-// * The name of Intel Corporation may not be used to endorse or promote products
-// derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
-#include "_cv.h"
-
-
-CV_IMPL CvKalman*
-cvCreateKalman( int DP, int MP, int CP )
-{
- CvKalman *kalman = 0;
-
- CV_FUNCNAME( "cvCreateKalman" );
-
- __BEGIN__;
-
- if( DP <= 0 || MP <= 0 )
- CV_ERROR( CV_StsOutOfRange,
- "state and measurement vectors must have positive number of dimensions" );
-
- if( CP < 0 )
- CP = DP;
-
- /* allocating memory for the structure */
- CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));
- memset( kalman, 0, sizeof(*kalman));
-
- kalman->DP = DP;
- kalman->MP = MP;
- kalman->CP = CP;
-
- CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));
- cvZero( kalman->state_pre );
-
- CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));
- cvZero( kalman->state_post );
-
- CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));
- cvSetIdentity( kalman->transition_matrix );
-
- CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));
- cvSetIdentity( kalman->process_noise_cov );
-
- CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));
- cvZero( kalman->measurement_matrix );
-
- CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));
- cvSetIdentity( kalman->measurement_noise_cov );
-
- CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));
-
- CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));
- cvZero( kalman->error_cov_post );
-
- CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));
-
- if( CP > 0 )
- {
- CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));
- cvZero( kalman->control_matrix );
- }
-
- CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));
- CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));
- CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));
- CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));
- CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));
-
-#if 1
- kalman->PosterState = kalman->state_pre->data.fl;
- kalman->PriorState = kalman->state_post->data.fl;
- kalman->DynamMatr = kalman->transition_matrix->data.fl;
- kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
- kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
- kalman->PNCovariance = kalman->process_noise_cov->data.fl;
- kalman->KalmGainMatr = kalman->gain->data.fl;
- kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
- kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
-#endif
-
- __END__;
-
- if( cvGetErrStatus() < 0 )
- cvReleaseKalman( &kalman );
-
- return kalman;
-}
-
-
-CV_IMPL void
-cvReleaseKalman( CvKalman** _kalman )
-{
- CvKalman *kalman;
-
- CV_FUNCNAME( "cvReleaseKalman" );
- __BEGIN__;
-
- if( !_kalman )
- CV_ERROR( CV_StsNullPtr, "" );
-
- kalman = *_kalman;
-
- /* freeing the memory */
- cvReleaseMat( &kalman->state_pre );
- cvReleaseMat( &kalman->state_post );
- cvReleaseMat( &kalman->transition_matrix );
- cvReleaseMat( &kalman->control_matrix );
- cvReleaseMat( &kalman->measurement_matrix );
- cvReleaseMat( &kalman->process_noise_cov );
- cvReleaseMat( &kalman->measurement_noise_cov );
- cvReleaseMat( &kalman->error_cov_pre );
- cvReleaseMat( &kalman->gain );
- cvReleaseMat( &kalman->error_cov_post );
- cvReleaseMat( &kalman->temp1 );
- cvReleaseMat( &kalman->temp2 );
- cvReleaseMat( &kalman->temp3 );
- cvReleaseMat( &kalman->temp4 );
- cvReleaseMat( &kalman->temp5 );
-
- memset( kalman, 0, sizeof(*kalman));
-
- /* deallocating the structure */
- cvFree( _kalman );
-
- __END__;
-}
-
-
-CV_IMPL const CvMat*
-cvKalmanPredict( CvKalman* kalman, const CvMat* control )
-{
- CvMat* result = 0;
-
- CV_FUNCNAME( "cvKalmanPredict" );
-
- __BEGIN__;
-
- if( !kalman )
- CV_ERROR( CV_StsNullPtr, "" );
-
- /* update the state */
- /* x'(k) = A*x(k) */
- CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));
-
- if( control && kalman->CP > 0 )
- /* x'(k) = x'(k) + B*u(k) */
- CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));
-
- /* update error covariance matrices */
- /* temp1 = A*P(k) */
- CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));
-
- /* P'(k) = temp1*At + Q */
- CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
- kalman->error_cov_pre, CV_GEMM_B_T ));
-
- result = kalman->state_pre;
-
- __END__;
-
- return result;
-}
-
-
-CV_IMPL const CvMat*
-cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
-{
- CvMat* result = 0;
-
- CV_FUNCNAME( "cvKalmanCorrect" );
-
- __BEGIN__;
-
- if( !kalman || !measurement )
- CV_ERROR( CV_StsNullPtr, "" );
-
- /* temp2 = H*P'(k) */
- CV_CALL( cvMatMulAdd( kalman->measurement_matrix,
- kalman->error_cov_pre, 0, kalman->temp2 ));
- /* temp3 = temp2*Ht + R */
- CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
- kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));
-
- /* temp4 = inv(temp3)*temp2 = Kt(k) */
- CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));
-
- /* K(k) */
- CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));
-
- /* temp5 = z(k) - H*x'(k) */
- CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));
-
- /* x(k) = x'(k) + K(k)*temp5 */
- CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));
-
- /* P(k) = P'(k) - K(k)*temp2 */
- CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
- kalman->error_cov_post, 0 ));
-
- result = kalman->state_post;
-
- __END__;
-
- return result;
-}
projects / opencv / blobdiff