卡尔曼滤波之Opencv（二）

今天研究了一下卡尔曼滤波跟踪，同时也看了一下卡尔曼滤波Opencv的源代码，总是看懂了，具体原理可以看看【1】。下面是opencv自带的一个程序，代码如下：

// kalman.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"


#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <stdio.h>

using namespace cv;

static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}

static void help()
{
    printf( "\nExamle of c calls to OpenCV's Kalman filter.\n"
"   Tracking of rotating point.\n"
"   Rotation speed is constant.\n"
"   Both state and measurements vectors are 1D (a point angle),\n"
"   Measurement is the real point angle + gaussian noise.\n"
"   The real and the estimated points are connected with yellow line segment,\n"
"   the real and the measured points are connected with red line segment.\n"
"   (if Kalman filter works correctly,\n"
"    the yellow segment should be shorter than the red one).\n"
            "\n"
"   Pressing any key (except ESC) will reset the tracking with a different speed.\n"
"   Pressing ESC will stop the program.\n"
            );
}

int main(int, char**)
{
    help();
    Mat img(500, 500, CV_8UC3);
    KalmanFilter KF(2, 1, 0);
	//[x1,x2]=[角度，角速度]
	/*
	运动模型：x1(k+1) = x1(k) + x2(k)*T
	         x2(k+1) = x2(k)
	状态转移方程：
	x^ = AX + w
	测量方程