← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/video/src/kalman.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/modules/video/src/kalman.cpp
 
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
}