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 "_cv.h"
42
43
44 CV_IMPL CvKalman*
cvCreateKalman(int DP,int MP,int CP)45 cvCreateKalman( int DP, int MP, int CP )
46 {
47 CvKalman *kalman = 0;
48
49 CV_FUNCNAME( "cvCreateKalman" );
50
51 __BEGIN__;
52
53 if( DP <= 0 || MP <= 0 )
54 CV_ERROR( CV_StsOutOfRange,
55 "state and measurement vectors must have positive number of dimensions" );
56
57 if( CP < 0 )
58 CP = DP;
59
60 /* allocating memory for the structure */
61 CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));
62 memset( kalman, 0, sizeof(*kalman));
63
64 kalman->DP = DP;
65 kalman->MP = MP;
66 kalman->CP = CP;
67
68 CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));
69 cvZero( kalman->state_pre );
70
71 CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));
72 cvZero( kalman->state_post );
73
74 CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));
75 cvSetIdentity( kalman->transition_matrix );
76
77 CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));
78 cvSetIdentity( kalman->process_noise_cov );
79
80 CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));
81 cvZero( kalman->measurement_matrix );
82
83 CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));
84 cvSetIdentity( kalman->measurement_noise_cov );
85
86 CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));
87
88 CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));
89 cvZero( kalman->error_cov_post );
90
91 CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));
92
93 if( CP > 0 )
94 {
95 CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));
96 cvZero( kalman->control_matrix );
97 }
98
99 CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));
100 CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));
101 CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));
102 CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));
103 CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));
104
105 #if 1
106 kalman->PosterState = kalman->state_pre->data.fl;
107 kalman->PriorState = kalman->state_post->data.fl;
108 kalman->DynamMatr = kalman->transition_matrix->data.fl;
109 kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
110 kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
111 kalman->PNCovariance = kalman->process_noise_cov->data.fl;
112 kalman->KalmGainMatr = kalman->gain->data.fl;
113 kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
114 kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
115 #endif
116
117 __END__;
118
119 if( cvGetErrStatus() < 0 )
120 cvReleaseKalman( &kalman );
121
122 return kalman;
123 }
124
125
126 CV_IMPL void
cvReleaseKalman(CvKalman ** _kalman)127 cvReleaseKalman( CvKalman** _kalman )
128 {
129 CvKalman *kalman;
130
131 CV_FUNCNAME( "cvReleaseKalman" );
132 __BEGIN__;
133
134 if( !_kalman )
135 CV_ERROR( CV_StsNullPtr, "" );
136
137 kalman = *_kalman;
138
139 /* freeing the memory */
140 cvReleaseMat( &kalman->state_pre );
141 cvReleaseMat( &kalman->state_post );
142 cvReleaseMat( &kalman->transition_matrix );
143 cvReleaseMat( &kalman->control_matrix );
144 cvReleaseMat( &kalman->measurement_matrix );
145 cvReleaseMat( &kalman->process_noise_cov );
146 cvReleaseMat( &kalman->measurement_noise_cov );
147 cvReleaseMat( &kalman->error_cov_pre );
148 cvReleaseMat( &kalman->gain );
149 cvReleaseMat( &kalman->error_cov_post );
150 cvReleaseMat( &kalman->temp1 );
151 cvReleaseMat( &kalman->temp2 );
152 cvReleaseMat( &kalman->temp3 );
153 cvReleaseMat( &kalman->temp4 );
154 cvReleaseMat( &kalman->temp5 );
155
156 memset( kalman, 0, sizeof(*kalman));
157
158 /* deallocating the structure */
159 cvFree( _kalman );
160
161 __END__;
162 }
163
164
165 CV_IMPL const CvMat*
cvKalmanPredict(CvKalman * kalman,const CvMat * control)166 cvKalmanPredict( CvKalman* kalman, const CvMat* control )
167 {
168 CvMat* result = 0;
169
170 CV_FUNCNAME( "cvKalmanPredict" );
171
172 __BEGIN__;
173
174 if( !kalman )
175 CV_ERROR( CV_StsNullPtr, "" );
176
177 /* update the state */
178 /* x'(k) = A*x(k) */
179 CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));
180
181 if( control && kalman->CP > 0 )
182 /* x'(k) = x'(k) + B*u(k) */
183 CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));
184
185 /* update error covariance matrices */
186 /* temp1 = A*P(k) */
187 CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));
188
189 /* P'(k) = temp1*At + Q */
190 CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
191 kalman->error_cov_pre, CV_GEMM_B_T ));
192
193 result = kalman->state_pre;
194
195 __END__;
196
197 return result;
198 }
199
200
201 CV_IMPL const CvMat*
cvKalmanCorrect(CvKalman * kalman,const CvMat * measurement)202 cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
203 {
204 CvMat* result = 0;
205
206 CV_FUNCNAME( "cvKalmanCorrect" );
207
208 __BEGIN__;
209
210 if( !kalman || !measurement )
211 CV_ERROR( CV_StsNullPtr, "" );
212
213 /* temp2 = H*P'(k) */
214 CV_CALL( cvMatMulAdd( kalman->measurement_matrix,
215 kalman->error_cov_pre, 0, kalman->temp2 ));
216 /* temp3 = temp2*Ht + R */
217 CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
218 kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));
219
220 /* temp4 = inv(temp3)*temp2 = Kt(k) */
221 CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));
222
223 /* K(k) */
224 CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));
225
226 /* temp5 = z(k) - H*x'(k) */
227 CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));
228
229 /* x(k) = x'(k) + K(k)*temp5 */
230 CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));
231
232 /* P(k) = P'(k) - K(k)*temp2 */
233 CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
234 kalman->error_cov_post, 0 ));
235
236 result = kalman->state_post;
237
238 __END__;
239
240 return result;
241 }
242