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
KalmanFilter()46 KalmanFilter::KalmanFilter() {}
KalmanFilter(int dynamParams,int measureParams,int controlParams,int type)47 KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
48 {
49 init(dynamParams, measureParams, controlParams, type);
50 }
51
init(int DP,int MP,int CP,int type)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
predict(const Mat & control)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
correct(const Mat & measurement)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