OpenCV4入门系列教程50:kalman(卡尔曼)滤波

入门系列索引:系列教程索引地址

上一篇:OpenCV4入门系列教程49:拉普拉斯算法锐化

卡尔曼滤波内容详见:图解卡尔曼滤波工作原理(翻译)

编程步骤

定义KalmanFilter类并初始化

KalmanFilter KF(DP, MP, 0);//构造KF对象

//初始化相关参数

KF.transitionMatrix 转移矩阵 A

KF.measurementMatrix 测量矩阵 H

KF.processNoiseCov 过程噪声 Q

KF.measurementNoiseCov 测量噪声 R

KF.errorCovPost 最小均方误差 P

KF.statePost 系统初始状态 x(0)

Mat measurement 定义初始测量值 z(0)

预测

KF.predict( ) //返回的是下一时刻的状态值KF.statePost (k+1)

更新

更新measurement; //注意measurement不能通过观测方程进行计算得到,要自己定义!

更新KF KF.correct(measurement)

最终的结果应该是更新后的statePost.

相关参数的确定

对于系统状态方程,简记为Y=AX+B,X和Y是表示系统状态的列向量,A是转移矩阵,B是其他项。

状态值(向量)只要能表示系统的状态即可,状态值的维数决定了转移矩阵A的维数,比如X和Y是N×1的,则A是N×N的。

A的确定跟X有关,只要保证方程中不相干项的系数为0即可,看下面例子

X和Y是二维的,

[cd]=[1001][cd]+B\left[ \begin{matrix} c \\ d \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 \\ 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \end{matrix} \right] + B

X和Y是三维的,

[cde]=[100010001][cde]+B\left[ \begin{matrix} c \\ d \\ e \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \\ e \end{matrix} \right] + B

X和Y是三维的,但c和Δc\Delta c是相关项

[cdΔc]=[101010001][cdΔc]+B\left[ \begin{matrix} c \\ d \\ \Delta c \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 && 1 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \\ \Delta c \end{matrix} \right] + B

[cΔcd]=[110010001][cΔcd]+B\left[ \begin{matrix} c \\ \Delta c \\ d \end{matrix} \right] = \left[ \begin{matrix} 1 && 1 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ \Delta c \\ d \end{matrix} \right] + B

上面的1也可以是其他值。

下面对predict( ) 和correct( )函数介绍下,可以不用看,不影响编程。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control){
gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
oclMat temp;

if(control.data)
gemm(controlMatrix, control, 1, statePre, 1, statePre);
gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
statePre.copyTo(statePost);
return statePre;
}

CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control){
gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
oclMat temp;

if(control.data)
gemm(controlMatrix, control, 1, statePre, 1, statePre);
gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
statePre.copyTo(statePost);
return statePre;
}

gemm( )是矩阵的广义乘法

1
2
3
4
5
6
7
8
void gemm(const GpuMat& src1,
constGpuMat& src2,
double alpha,
const GpuMat& src3,
double beta,
GpuMat& dst,
int flags=0,
Stream& stream=Stream::Null())

$ dst = alpha · src1 · src2 +beta· src3 $

上面,oclMat()其实是uku_k,只不过默认为0,所以没赋值。整个过程就计算了x’和P’。(用x’代表x的预测值,用P’代表P的预测值)。GEMM_2_T表示对第2个参数转置。

x(k)=1Ax(k1)x'(k)=1·A·x(k-1)

如果B非空,

x’(k) = 1·B·u + 1·x’(k-1)
temp1 = 1·A·P(k-1) + 0·u(k)
P’(k) = 1· temp1·AT + 1· Qk= A·P(k-1)·AT + 1· Qk

可见,和第一部分的理论介绍完全一致。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
CV_EXPORTS const oclMat& KalmanFilter::correct(const oclMat& measurement)
{
CV_Assert(measurement.empty() == false);
gemm(measurementMatrix, errorCovPre, 1, oclMat(), 0, temp2);
gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
Mat temp;
solve(Mat(temp3), Mat(temp2), temp, DECOMP_SVD);
temp4.upload(temp);
gain = temp4.t();
gemm(measurementMatrix, statePre, -1, measurement, 1, temp5);
gemm(gain, temp5, 1, statePre, 1, statePost);
gemm(gain, temp2, -1, errorCovPre, 1, errorCovPost);
return statePost;
}
1
2
3
4
bool solve(InputArray src1,
InputArray src2,
OutputArray dst,
int flags=DECOMP_LU)

求解线型最小二乘估计

temp2 = 1· H·P’ + 0·u(k)
temp3 = 1· temp2·HT + 1·R = H·P’·HT+ 1· R 也就是上面的Sk
temp = argmin||tem2- temp3||
K=temp
temp5 = -1· H·x’ + 1·zk 就是上面的y’。
x = 1·K·temp5 + 1·x’ = KT·y’ +x’
P =-1·K·temp2 + 1·P’ = -K·H·P’+P’ = (I- K·H) P’

也和第一部分的理论完全一致。

通过深入函数内部,学到了两个实用的函数哦。矩阵广义乘法gemm( )、最小二乘估计solve( )

测试代码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
//---------------------------------【头文件、命名空间包含部分】----------------------------
// 描述:包含程序所使用的头文件和命名空间
//------------------------------------------------------------------------------------------------
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include
using namespace cv;

//---------------------------------【calcPoint( )函数】--------------------------------------
// 描述:计算点坐标
//----------------------------------------------------------------------------------------------
static inline Point calcPoint(Point2f center, double R, double angle) {
return center + Point2f((float)cos(angle), (float)-sin(angle)) * (float)R;
}

//--------------------------------------【help( )函数】--------------------------------------
// 描述:输出一些帮助信息
//----------------------------------------------------------------------------------------------
static void help() {
printf("\n\t用OpenCV动态绘制卡尔曼滤波。\n");
printf("\n\t任意按键重置轨迹,并更新速度\n");
printf("\n\tESC键-程序结束\n");
}

//-----------------------------------【main( )函数】--------------------------------------------
// 描述:控制台应用程序的入口函数,我们的程序从这里开始
//-------------------------------------------------------------------------------------------------
int main(int, char **) {
help();
Mat img(500, 500, CV_8UC3);
KalmanFilter KF(2, 1, 0);
Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F);
char code = (char)-1;

for (;;) {
randn(state, Scalar::all(0), Scalar::all(0.1));
KF.transitionMatrix = Mat_(2, 2) << 1, 1, 0, 1;

setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(1));

randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

for (;;) {
Point2f center(img.cols * 0.5f, img.rows * 0.5f);
float R = img.cols / 3.f;
double stateAngle = state.at(0);
Point statePt = calcPoint(center, R, stateAngle);

Mat prediction = KF.predict();
double predictAngle = prediction.at(0);
Point predictPt = calcPoint(center, R, predictAngle);

randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at(0)));

// generate measurement
measurement += KF.measurementMatrix * state;

double measAngle = measurement.at(0);
Point measPt = calcPoint(center, R, measAngle);

// plot points
#define drawCross(center, color, d) \
line(img, Point(center.x - d, center.y - d), Point(center.x + d, center.y + d), color, 1, \
LINE_AA, 0); \
line(img, Point(center.x + d, center.y - d), Point(center.x - d, center.y + d), color, 1, \
LINE_AA, 0)

img = Scalar::all(0);
drawCross(statePt, Scalar(255, 255, 255), 3);
drawCross(measPt, Scalar(0, 0, 255), 3);
drawCross(predictPt, Scalar(0, 255, 0), 3);
line(img, statePt, measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
line(img, statePt, predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);

if (theRNG().uniform(0, 4) != 0)
KF.correct(measurement);

randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at(0, 0))));
state = KF.transitionMatrix * state + processNoise;

imshow("Kalman", img);
code = (char)waitKey(100);

if (code > 0)
break;
}
if (code == 27 || code == 'q' || code == 'Q')
break;
}

return 0;
}

效果:

kalman

下一篇:OpenCV4入门系列教程51:filter2D(图像掩码与滤波)


本博客所有文章除特别声明外,均采用 CC BY-SA 4.0 协议 ,转载请注明出处!