1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // Intel License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Third party copyrights are property of their respective owners. 15 // 16 // Redistribution and use in source and binary forms, with or without modification, 17 // are permitted provided that the following conditions are met: 18 // 19 // * Redistribution's of source code must retain the above copyright notice, 20 // this list of conditions and the following disclaimer. 21 // 22 // * Redistribution's in binary form must reproduce the above copyright notice, 23 // this list of conditions and the following disclaimer in the documentation 24 // and/or other materials provided with the distribution. 25 // 26 // * The name of Intel Corporation may not be used to endorse or promote products 27 // derived from this software without specific prior written permission. 28 // 29 // This software is provided by the copyright holders and contributors "as is" and 30 // any express or implied warranties, including, but not limited to, the implied 31 // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 // In no event shall the Intel Corporation or contributors be liable for any direct, 33 // indirect, incidental, special, exemplary, or consequential damages 34 // (including, but not limited to, procurement of substitute goods or services; 35 // loss of use, data, or profits; or business interruption) however caused 36 // and on any theory of liability, whether in contract, strict liability, 37 // or tort (including negligence or otherwise) arising in any way out of 38 // the use of this software, even if advised of the possibility of such damage. 39 // 40 //M*/ 41 #include "precomp.hpp" 42 43 namespace cv 44 { 45 46 KalmanFilter::KalmanFilter() {} 47 KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type) 48 { 49 init(dynamParams, measureParams, controlParams, type); 50 } 51 52 void KalmanFilter::init(int DP, int MP, int CP, int type) 53 { 54 CV_Assert( DP > 0 && MP > 0 ); 55 CV_Assert( type == CV_32F || type == CV_64F ); 56 CP = std::max(CP, 0); 57 58 statePre = Mat::zeros(DP, 1, type); 59 statePost = Mat::zeros(DP, 1, type); 60 transitionMatrix = Mat::eye(DP, DP, type); 61 62 processNoiseCov = Mat::eye(DP, DP, type); 63 measurementMatrix = Mat::zeros(MP, DP, type); 64 measurementNoiseCov = Mat::eye(MP, MP, type); 65 66 errorCovPre = Mat::zeros(DP, DP, type); 67 errorCovPost = Mat::zeros(DP, DP, type); 68 gain = Mat::zeros(DP, MP, type); 69 70 if( CP > 0 ) 71 controlMatrix = Mat::zeros(DP, CP, type); 72 else 73 controlMatrix.release(); 74 75 temp1.create(DP, DP, type); 76 temp2.create(MP, DP, type); 77 temp3.create(MP, MP, type); 78 temp4.create(MP, DP, type); 79 temp5.create(MP, 1, type); 80 } 81 82 const Mat& KalmanFilter::predict(const Mat& control) 83 { 84 // update the state: x'(k) = A*x(k) 85 statePre = transitionMatrix*statePost; 86 87 if( !control.empty() ) 88 // x'(k) = x'(k) + B*u(k) 89 statePre += controlMatrix*control; 90 91 // update error covariance matrices: temp1 = A*P(k) 92 temp1 = transitionMatrix*errorCovPost; 93 94 // P'(k) = temp1*At + Q 95 gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T); 96 97 // handle the case when there will be measurement before the next predict. 98 statePre.copyTo(statePost); 99 errorCovPre.copyTo(errorCovPost); 100 101 return statePre; 102 } 103 104 const Mat& KalmanFilter::correct(const Mat& measurement) 105 { 106 // temp2 = H*P'(k) 107 temp2 = measurementMatrix * errorCovPre; 108 109 // temp3 = temp2*Ht + R 110 gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T); 111 112 // temp4 = inv(temp3)*temp2 = Kt(k) 113 solve(temp3, temp2, temp4, DECOMP_SVD); 114 115 // K(k) 116 gain = temp4.t(); 117 118 // temp5 = z(k) - H*x'(k) 119 temp5 = measurement - measurementMatrix*statePre; 120 121 // x(k) = x'(k) + K(k)*temp5 122 statePost = statePre + gain*temp5; 123 124 // P(k) = P'(k) - K(k)*temp2 125 errorCovPost = errorCovPre - gain*temp2; 126 127 return statePost; 128 } 129 130 } 131
