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 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2015, OpenCV Foundation, 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 the copyright holders 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
42 #include "precomp.hpp"
43 #include "opencv2/tracking/kalman_filters.hpp"
44
45 namespace cv {
46 namespace detail {
47 inline namespace tracking {
48 inline namespace kalman_filters {
49
50 void UnscentedKalmanFilterParams::
init(int dp,int mp,int cp,double processNoiseCovDiag,double measurementNoiseCovDiag,Ptr<UkfSystemModel> dynamicalSystem,int type)51 init( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
52 Ptr<UkfSystemModel> dynamicalSystem, int type )
53 {
54 CV_Assert( dp > 0 && mp > 0 );
55 DP = dp;
56 MP = mp;
57 CP = std::max( cp, 0 );
58 CV_Assert( type == CV_32F || type == CV_64F );
59 dataType = type;
60
61 this->model = dynamicalSystem;
62
63 stateInit = Mat::zeros( DP, 1, type );
64 errorCovInit = Mat::eye( DP, DP, type );
65
66 processNoiseCov = processNoiseCovDiag*Mat::eye( DP, DP, type );
67 measurementNoiseCov = measurementNoiseCovDiag*Mat::eye( MP, MP, type );
68
69 alpha = 1e-3;
70 k = 0.0;
71 beta = 2.0;
72 }
73
74 UnscentedKalmanFilterParams::
UnscentedKalmanFilterParams(int dp,int mp,int cp,double processNoiseCovDiag,double measurementNoiseCovDiag,Ptr<UkfSystemModel> dynamicalSystem,int type)75 UnscentedKalmanFilterParams( int dp, int mp, int cp, double processNoiseCovDiag, double measurementNoiseCovDiag,
76 Ptr<UkfSystemModel> dynamicalSystem, int type )
77 {
78 init( dp, mp, cp, processNoiseCovDiag, measurementNoiseCovDiag, dynamicalSystem, type );
79 }
80
81 class UnscentedKalmanFilterImpl: public UnscentedKalmanFilter
82 {
83
84 int DP; // dimensionality of the state vector
85 int MP; // dimensionality of the measurement vector
86 int CP; // dimensionality of the control vector
87 int dataType; // type of elements of vectors and matrices
88
89 Mat state; // estimate of the system state (x*), DP x 1
90 Mat errorCov; // estimate of the state cross-covariance matrix (P), DP x DP
91
92 Mat processNoiseCov; // process noise cross-covariance matrix (Q), DP x DP
93 Mat measurementNoiseCov; // measurement noise cross-covariance matrix (R), MP x MP
94
95 Ptr<UkfSystemModel> model; // object of the class containing functions for computing the next state and the measurement.
96
97 // Parameters of algorithm
98 double alpha; // parameter, default is 1e-3
99 double k; // parameter, default is 0
100 double beta; // parameter, default is 2.0
101
102 double lambda; // internal parameter, lambda = alpha*alpha*( DP + k ) - DP;
103 double tmpLambda; // internal parameter, tmpLambda = alpha*alpha*( DP + k );
104
105 // Auxillary members
106 Mat measurementEstimate; // estimate of current measurement (y*), MP x 1
107
108 Mat sigmaPoints; // set of sigma points ( x_i, i = 1..2*DP+1 ), DP x 2*DP+1
109
110 Mat transitionSPFuncVals; // set of state function values at sigma points ( f_i, i = 1..2*DP+1 ), DP x 2*DP+1
111 Mat measurementSPFuncVals; // set of measurement function values at sigma points ( h_i, i = 1..2*DP+1 ), MP x 2*DP+1
112
113 Mat transitionSPFuncValsCenter; // set of state function values at sigma points minus estimate of state ( fc_i, i = 1..2*DP+1 ), DP x 2*DP+1
114 Mat measurementSPFuncValsCenter; // set of measurement function values at sigma points minus estimate of measurement ( hc_i, i = 1..2*DP+1 ), MP x 2*DP+1
115
116 Mat Wm; // vector of weights for estimate mean, 2*DP+1 x 1
117 Mat Wc; // matrix of weights for estimate covariance, 2*DP+1 x 2*DP+1
118
119 Mat gain; // Kalman gain matrix (K), DP x MP
120 Mat xyCov; // estimate of the covariance between x* and y* (Sxy), DP x MP
121 Mat yyCov; // estimate of the y* cross-covariance matrix (Syy), MP x MP
122
123 Mat r; // zero vector of process noise for getting transitionSPFuncVals,
124 Mat q; // zero vector of measurement noise for getting measurementSPFuncVals
125
126 Mat getSigmaPoints( const Mat& mean, const Mat& covMatrix, double coef );
127
128 public:
129
130 UnscentedKalmanFilterImpl( const UnscentedKalmanFilterParams& params );
131 ~UnscentedKalmanFilterImpl();
132
133 // perform prediction step
134 // control - the optional control vector, CP x 1
135 Mat predict( InputArray control = noArray() ) CV_OVERRIDE;
136
137 // perform correction step
138 // measurement - current measurement vector, MP x 1
139 Mat correct( InputArray measurement ) CV_OVERRIDE;
140
141 // Get system parameters
142 Mat getProcessNoiseCov() const CV_OVERRIDE;
143 Mat getMeasurementNoiseCov() const CV_OVERRIDE;
144 Mat getErrorCov() const CV_OVERRIDE;
145
146 // Get the state estimate
147 Mat getState() const CV_OVERRIDE;
148 };
149
UnscentedKalmanFilterImpl(const UnscentedKalmanFilterParams & params)150 UnscentedKalmanFilterImpl::UnscentedKalmanFilterImpl(const UnscentedKalmanFilterParams& params)
151 {
152 alpha = params.alpha;
153 beta = params.beta;
154 k = params.k;
155
156 CV_Assert( params.DP > 0 && params.MP > 0 );
157 CV_Assert( params.dataType == CV_32F || params.dataType == CV_64F );
158 DP = params.DP;
159 MP = params.MP;
160 CP = std::max( params.CP, 0 );
161 dataType = params.dataType;
162
163 model = params.model;
164
165 CV_Assert( params.stateInit.cols == 1 && params.stateInit.rows == DP );
166 CV_Assert( params.errorCovInit.cols == DP && params.errorCovInit.rows == DP );
167 state = params.stateInit.clone();
168 errorCov = params.errorCovInit.clone();
169
170 CV_Assert( params.processNoiseCov.cols == DP && params.processNoiseCov.rows == DP );
171 CV_Assert( params.measurementNoiseCov.cols == MP && params.measurementNoiseCov.rows == MP );
172 processNoiseCov = params.processNoiseCov.clone();
173 measurementNoiseCov = params.measurementNoiseCov.clone();
174
175 measurementEstimate = Mat::zeros( MP, 1, dataType);
176
177 q = Mat::zeros( DP, 1, dataType);
178 r = Mat::zeros( MP, 1, dataType);
179
180 gain = Mat::zeros( DP, DP, dataType );
181
182 transitionSPFuncVals = Mat::zeros( DP, 2*DP+1, dataType );
183 measurementSPFuncVals = Mat::zeros( MP, 2*DP+1, dataType );
184
185 transitionSPFuncValsCenter = Mat::zeros( DP, 2*DP+1, dataType );
186 measurementSPFuncValsCenter = Mat::zeros( MP, 2*DP+1, dataType );
187
188 lambda = alpha*alpha*( DP + k ) - DP;
189 tmpLambda = lambda + DP;
190
191 double tmp2Lambda = 0.5/tmpLambda;
192
193 Wm = tmp2Lambda * Mat::ones( 2*DP+1, 1, dataType );
194 Wc = tmp2Lambda * Mat::eye( 2*DP+1, 2*DP+1, dataType );
195
196 if ( dataType == CV_64F )
197 {
198 Wm.at<double>(0,0) = lambda/tmpLambda;
199 Wc.at<double>(0,0) = lambda/tmpLambda + 1.0 - alpha*alpha + beta;
200 }
201 else
202 {
203 Wm.at<float>(0,0) = (float)(lambda/tmpLambda);
204 Wc.at<float>(0,0) = (float)(lambda/tmpLambda + 1.0 - alpha*alpha + beta);
205 }
206 }
207
~UnscentedKalmanFilterImpl()208 UnscentedKalmanFilterImpl::~UnscentedKalmanFilterImpl()
209 {
210 state.release();
211 errorCov.release();
212
213 processNoiseCov.release();
214 measurementNoiseCov.release();
215
216 measurementEstimate.release();
217
218 sigmaPoints.release();
219
220 transitionSPFuncVals.release();
221 measurementSPFuncVals.release();
222
223 transitionSPFuncValsCenter.release();
224 measurementSPFuncValsCenter.release();
225
226 Wm.release();
227 Wc.release();
228
229 gain.release();
230 xyCov.release();
231 yyCov.release();
232
233 r.release();
234 q.release();
235 }
236
getSigmaPoints(const Mat & mean,const Mat & covMatrix,double coef)237 Mat UnscentedKalmanFilterImpl::getSigmaPoints(const Mat &mean, const Mat &covMatrix, double coef)
238 {
239 // x_0 = mean
240 // x_i = mean + coef * cholesky( covMatrix ), i = 1..n
241 // x_(i+n) = mean - coef * cholesky( covMatrix ), i = 1..n
242
243 int n = mean.rows;
244 Mat points = repeat(mean, 1, 2*n+1);
245
246 Mat covMatrixL = covMatrix.clone();
247
248 // covMatrixL = cholesky( covMatrix )
249 if ( dataType == CV_64F )
250 choleskyDecomposition<double>(
251 covMatrix.ptr<double>(), covMatrix.step, covMatrix.rows,
252 covMatrixL.ptr<double>(), covMatrixL.step );
253 else if ( dataType == CV_32F )
254 choleskyDecomposition<float>(
255 covMatrix.ptr<float>(), covMatrix.step, covMatrix.rows,
256 covMatrixL.ptr<float>(), covMatrixL.step );
257
258 covMatrixL = coef * covMatrixL;
259
260 Mat p_plus = points( Rect( 1, 0, n, n ) );
261 Mat p_minus = points( Rect( n+1, 0, n, n ) );
262
263 add(p_plus, covMatrixL, p_plus);
264 subtract(p_minus, covMatrixL, p_minus);
265
266 return points;
267 }
268
predict(InputArray _control)269 Mat UnscentedKalmanFilterImpl::predict(InputArray _control)
270 {
271 Mat control = _control.getMat();
272 // get sigma points from x* and P
273 sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) );
274
275 // compute f-function values at sigma points
276 // f_i = f(x_i, control, 0), i = 0..2*DP
277 Mat x, fx;
278 for ( int i = 0; i<2*DP+1; i++)
279 {
280 x = sigmaPoints( Rect( i, 0, 1, DP) );
281 fx = transitionSPFuncVals( Rect( i, 0, 1, DP) );
282 model->stateConversionFunction( x, control, q, fx );
283 }
284 // compute the estimate of state as mean f-function value at sigma point
285 // x* = SUM_{i=0}^{2*DP}( Wm[i]*f_i )
286 state = transitionSPFuncVals * Wm;
287
288 // compute f-function values at sigma points minus estimate of state
289 // fc_i = f_i - x*, i = 0..2*DP
290 subtract(transitionSPFuncVals, repeat( state, 1, 2*DP+1 ), transitionSPFuncValsCenter);
291
292 // compute the estimate of the state cross-covariance matrix
293 // P = SUM_{i=0}^{2*DP}( Wc[i]*fc_i*fc_i.t ) + Q
294 errorCov = transitionSPFuncValsCenter * Wc * transitionSPFuncValsCenter.t() + processNoiseCov;
295
296 return state.clone();
297 }
298
correct(InputArray _measurement)299 Mat UnscentedKalmanFilterImpl::correct(InputArray _measurement)
300 {
301 Mat measurement = _measurement.getMat();
302 // get sigma points from x* and P
303 sigmaPoints = getSigmaPoints( state, errorCov, sqrt( tmpLambda ) );
304
305 // compute h-function values at sigma points
306 // h_i = h(x_i, 0), i = 0..2*DP
307 Mat x, hx;
308 for ( int i = 0; i<2*DP+1; i++)
309 {
310 x = sigmaPoints( Rect( i, 0, 1, DP) );
311 hx = measurementSPFuncVals( Rect( i, 0, 1, MP) );
312 model->measurementFunction( x, r, hx );
313 }
314
315 // compute the estimate of measurement as mean h-function value at sigma point
316 // y* = SUM_{i=0}^{2*DP}( Wm[i]*h_i )
317 measurementEstimate = measurementSPFuncVals * Wm;
318
319 // compute h-function values at sigma points minus estimate of state
320 // hc_i = h_i - y*, i = 0..2*DP
321 subtract(measurementSPFuncVals, repeat( measurementEstimate, 1, 2*DP+1 ), measurementSPFuncValsCenter);
322
323 // compute the estimate of the y* cross-covariance matrix
324 // Syy = SUM_{i=0}^{2*DP}( Wc[i]*hc_i*hc_i.t ) + R
325 yyCov = measurementSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t() + measurementNoiseCov;
326
327 // compute the estimate of the covariance between x* and y*
328 // Sxy = SUM_{i=0}^{2*DP}( Wc[i]*fc_i*hc_i.t )
329 xyCov = transitionSPFuncValsCenter * Wc * measurementSPFuncValsCenter.t();
330
331 // compute the Kalman gain matrix
332 // K = Sxy * Syy^(-1)
333 gain = xyCov * yyCov.inv(DECOMP_SVD);
334
335 // compute the corrected estimate of state
336 // x* = x* + K*(y - y*), y - current measurement
337 state = state + gain * ( measurement - measurementEstimate );
338
339 // compute the corrected estimate of the state cross-covariance matrix
340 // P = P - K*Sxy.t
341 errorCov = errorCov - gain * xyCov.t();
342
343 return state.clone();
344 }
345
getProcessNoiseCov() const346 Mat UnscentedKalmanFilterImpl::getProcessNoiseCov() const
347 {
348 return processNoiseCov.clone();
349 }
350
getMeasurementNoiseCov() const351 Mat UnscentedKalmanFilterImpl::getMeasurementNoiseCov() const
352 {
353 return measurementNoiseCov.clone();
354 }
355
getErrorCov() const356 Mat UnscentedKalmanFilterImpl::getErrorCov() const
357 {
358 return errorCov.clone();
359 }
360
getState() const361 Mat UnscentedKalmanFilterImpl::getState() const
362 {
363 return state.clone();
364 }
365
createUnscentedKalmanFilter(const UnscentedKalmanFilterParams & params)366 Ptr<UnscentedKalmanFilter> createUnscentedKalmanFilter(const UnscentedKalmanFilterParams ¶ms)
367 {
368 Ptr<UnscentedKalmanFilter> kfu( new UnscentedKalmanFilterImpl(params) );
369 return kfu;
370 }
371
372 }}}} // namespace
373