16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvKalman* 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCreateKalman( int DP, int MP, int CP ) 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvKalman *kalman = 0; 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCreateKalman" ); 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( DP <= 0 || MP <= 0 ) 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "state and measurement vectors must have positive number of dimensions" ); 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CP < 0 ) 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CP = DP; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* allocating memory for the structure */ 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman ))); 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( kalman, 0, sizeof(*kalman)); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->DP = DP; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->MP = MP; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->CP = CP; 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 )); 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( kalman->state_pre ); 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 )); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( kalman->state_post ); 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 )); 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( kalman->transition_matrix ); 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 )); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( kalman->process_noise_cov ); 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 )); 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( kalman->measurement_matrix ); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 )); 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( kalman->measurement_noise_cov ); 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 )); 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 )); 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( kalman->error_cov_post ); 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 )); 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CP > 0 ) 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 )); 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( kalman->control_matrix ); 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 )); 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 )); 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 )); 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 )); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 )); 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->PosterState = kalman->state_pre->data.fl; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->PriorState = kalman->state_post->data.fl; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->DynamMatr = kalman->transition_matrix->data.fl; 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->MeasurementMatr = kalman->measurement_matrix->data.fl; 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->MNCovariance = kalman->measurement_noise_cov->data.fl; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->PNCovariance = kalman->process_noise_cov->data.fl; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->KalmGainMatr = kalman->gain->data.fl; 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl; 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl; 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvGetErrStatus() < 0 ) 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseKalman( &kalman ); 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return kalman; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvReleaseKalman( CvKalman** _kalman ) 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvKalman *kalman; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvReleaseKalman" ); 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !_kalman ) 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman = *_kalman; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* freeing the memory */ 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->state_pre ); 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->state_post ); 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->transition_matrix ); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->control_matrix ); 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->measurement_matrix ); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->process_noise_cov ); 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->measurement_noise_cov ); 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->error_cov_pre ); 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->gain ); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->error_cov_post ); 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->temp1 ); 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->temp2 ); 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->temp3 ); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->temp4 ); 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &kalman->temp5 ); 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( kalman, 0, sizeof(*kalman)); 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* deallocating the structure */ 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( _kalman ); 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL const CvMat* 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvKalmanPredict( CvKalman* kalman, const CvMat* control ) 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* result = 0; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvKalmanPredict" ); 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !kalman ) 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update the state */ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* x'(k) = A*x(k) */ 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre )); 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( control && kalman->CP > 0 ) 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* x'(k) = x'(k) + B*u(k) */ 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre )); 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* update error covariance matrices */ 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* temp1 = A*P(k) */ 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 )); 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* P'(k) = temp1*At + Q */ 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1, 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->error_cov_pre, CV_GEMM_B_T )); 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = kalman->state_pre; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL const CvMat* 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvKalmanCorrect( CvKalman* kalman, const CvMat* measurement ) 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* result = 0; 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvKalmanCorrect" ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !kalman || !measurement ) 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* temp2 = H*P'(k) */ 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( kalman->measurement_matrix, 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->error_cov_pre, 0, kalman->temp2 )); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* temp3 = temp2*Ht + R */ 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1, 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T )); 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* temp4 = inv(temp3)*temp2 = Kt(k) */ 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD )); 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* K(k) */ 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvTranspose( kalman->temp4, kalman->gain )); 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* temp5 = z(k) - H*x'(k) */ 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 )); 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* x(k) = x'(k) + K(k)*temp5 */ 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post )); 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* P(k) = P'(k) - K(k)*temp2 */ 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1, 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn kalman->error_cov_post, 0 )); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = kalman->state_post; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 242