[学习slam]基于opencv卡尔曼滤波(KalmanFilter)理论与实践

参考 https://en.wikipedia.org/wiki/Kalman_filter

背景：

      卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。

        目前,卡尔曼滤波已经有很多不同的实现.卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

模型：

基本动态系统模型:

卡尔曼滤波建立在线性代数和隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声(即正态分布的噪声)干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。 随着离散时间的每一个增加,这个线性算子就会作用在当前状态上，产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

简介：

kalman滤波简单介绍

     Kalman滤波理论主要应用在现实世界中个，并不是理想环境。主要是来跟踪的某一个变量的值，跟踪的依据是首先根据系统的运动方程来对该值做预测，比如说我们知道一个物体的运动速度，那么下面时刻它的位置按照道理是可以预测出来的，不过该预测肯定有误差，只能作为跟踪的依据。另一个依据是可以用测量手段来测量那个变量的值，当然该测量也是有误差的，也只能作为依据，不过这2个依据的权重比例不同。最后kalman滤波就是利用这两个依据进行一些列迭代进行目标跟踪的。

     在这个理论框架中，有2个公式一定要懂，即：

　　

　　

     第一个方程为系统的运动方程，第二个方程为系统的观测方程，学过自控原理中的现代控制理论的同学应该对这2个公式很熟悉。具体的相关理论本文就不做介绍了。

尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，符合下式：（A --Fk）

           

其中

• Fk 是作用在xk−1上的状态变换模型（/矩阵/矢量）。 
• Bk 是作用在控制器向量uk上的输入－控制模型。 
• wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

时刻k，对真实状态 xk的一个测量zk满足下式:

            

其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ...vk} 都认为是互相独立的.

卡尔曼滤波器：

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

卡尔曼滤波器的状态由以下两个变量表示：

• ，在时刻k 的状态的估计；
• ，误差相关矩阵，度量估计值的精确程度。

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测 predict:      Mat prediction = KF.predict();

 (预测状态)

 (预测估计协方差矩阵)

可参考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新 updata:     KF.correct(measurement);

 (测量余量，measurement residual)

 (测量余量协方差)

 (最优卡尔曼增益)

 (更新的状态估计)

 (更新的协方差估计)

使用上述公式计算仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。

不变量(Invariant)

如果模型准确，而且与 的值准确的反映了最初状态的分布，那么以下不变量就保持不变：所有估计的误差均值为零

• 
• 

且 协方差矩阵 准确的反映了估计的协方差:

• 
• 
• 

请注意，其中表示的期望值,。

PS:

估计噪声协方差Q k和R k

卡尔曼滤波器的实际实现通常是困难的，因为难以获得噪声协方差矩阵Q k和R k的良好估计。 在该领域已经进行了广泛的研究以从数据估计这些协方差。 实现此目的的一种实用方法是自协方差最小二乘（ALS）技术，该技术使用常规操作数据的时间滞后自协方差来估计协方差。 使用ALS技术计算噪声协方差矩阵的GNU Octave和Matlab代码可在GNU通用公共许可证下在线获得 。

autocovariance least-squares (ALS):在概率论和统计学中 ，给定一个随机过程 ， 自协方差是一个函数，它在成对的时间点给出了过程与自身的协方差 。 自协方差与所讨论过程的自相关密切相关。

"Autocovariance Least-Squares Toolbox". Jbrwww.che.wisc.edu. Retrieved 2014-06-02.

实践：

     Opencv目标版本中带有kalman这个类，可以使用它来完成一些跟踪目的。

例如：KalmanFilter KF(stateNum, measureNum, 0);

Fk : KF.transitionMatrix

Hk : KF.measurementMatrix

Qk : KF.processNoiseCov

Rk  : KF.measurementNoiseCov

Pk : KF.errorCovPost

有时也需要定义Bk : KF.controlMatrix

 

下面来看看使用Kalman编程的主要步骤：

　　步骤一  ：

     Kalman这个类需要初始化下面变量：      

　　转移矩阵，测量矩阵，控制向量(没有的话，就是0)，过程噪声协方差矩阵，测量噪声协方差矩阵，后验错误协方差矩阵，前一状态校正后的值，当前观察值。 

　　步骤二：

　　调用kalman这个类的predict方法得到状态的预测值矩阵，预测状态的计算公式如下：

　　predicted state (x'(k)): x'(k)=A*x(k-1)+B*u(k)

　　其中x(k-1)为前一状态的校正值，第一个循环中在初始化过程中已经给定了，后面的循环中Kalman这个类内部会计算。A,B,u(k),也都是给定了的值。这样进过计算就得到了系统状态的预测值x'(k)了。 

　　步骤三：

　　调用kalman这个类的correct方法得到加入观察值校正后的状态变量值矩阵，其公式为：

　　corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))

　　其中x'(k)为步骤二算出的结果，z(k)为当前测量值，是我们外部测量后输入的向量。H为Kalman类初始化给定的测量矩阵。K(k)为Kalman增益，其计算公式为：

　　Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)

　　计算该增益所依赖的变量要么初始化中给定，要么在kalman理论中通过其它公式可以计算。

　　经过步骤三后，我们又重新获得了这一时刻的校正值，后面就不断循环步骤二和步骤三即可完成Kalman滤波过程。

 

 

http://opencvexamples.blogspot.com/2014/01/kalman-filter-implementation-tracking.html

(Tracking mouse position)

KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)

Parameters:	dynamParams – Dimensionality of the state. measureParams – Dimensionality of the measurement. controlParams – Dimensionality of the control vector. type – Type of the created matrices that should be CV_32F or CV_64F.

const Mat& KalmanFilter::predict(const Mat& control=Mat()) 

   - Computes a predicted state

const Mat& KalmanFilter::correct(const Mat& measurement)

  - Updates the predicted state from the measurement.
动态参数——状态的维度。
测量参数——测量的维度。
ControlParams–控制向量的维数。
类型–创建的矩阵的类型，应为cv f或cv f。

Example:

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include 
 
#define drawCross( center, color, d )                                 \
line( img, Point( center.x - d, center.y - d ), Point( center.x + d, center.y + d ), color, 2, CV_AA, 0); \
line( img, Point( center.x + d, center.y - d ), Point( center.x - d, center.y + d ), color, 2, CV_AA, 0 )
 
using namespace cv;
using namespace std;
  
int main( )
{ 
 
KalmanFilter KF(4, 2, 0);
POINT mousePos;
GetCursorPos(&mousePos);
 
// intialization of KF...
KF.transitionMatrix = *(Mat_(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
Mat_ measurement(2,1); measurement.setTo(Scalar(0));
 
KF.statePre.at(0) = mousePos.x;
KF.statePre.at(1) = mousePos.y;
KF.statePre.at(2) = 0;
KF.statePre.at(3) = 0;
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
setIdentity(KF.measurementNoiseCov, Scalar::all(10));
setIdentity(KF.errorCovPost, Scalar::all(.1));
// Image to show mouse tracking
Mat img(600, 800, CV_8UC3);
vector mousev,kalmanv;
mousev.clear();
kalmanv.clear();
 
while(1)
{
 // First predict, to update the internal statePre variable
 Mat prediction = KF.predict();
 Point predictPt(prediction.at(0),prediction.at(1));
              
 // Get mouse point
 GetCursorPos(&mousePos);
 measurement(0) = mousePos.x;
 measurement(1) = mousePos.y; 
  
 // The update phase 
 Mat estimated = KF.correct(measurement);
 
 Point statePt(estimated.at(0),estimated.at(1));
 Point measPt(measurement(0),measurement(1));
    // plot points
    imshow("mouse kalman", img);
    img = Scalar::all(0);
 
    mousev.push_back(measPt);
    kalmanv.push_back(statePt);
    drawCross( statePt, Scalar(255,255,255), 5 );
    drawCross( measPt, Scalar(0,0,255), 5 );
 
    for (int i = 0; i < mousev.size()-1; i++) 
     line(img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
     
    for (int i = 0; i < kalmanv.size()-1; i++) 
     line(img, kalmanv[i], kalmanv[i+1], Scalar(0,155,255), 1);
     
 waitKey(10);  
}
                                           
    return 0;
}

同样类似的例子：

//
例2  跟踪鼠标位置
#include 
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include 
using namespace cv;
using namespace std;

const int winHeight=600;
const int winWidth=800;


Point mousePosition= Point(winWidth>>1,winHeight>>1);

//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param )
{
    if (event==CV_EVENT_MOUSEMOVE) {
        mousePosition = Point(x,y);
    }
}

int main (void)
{
    RNG rng;
    //1.kalman filter setup
    const int stateNum=4;                                      //状态值4×1向量(x,y,△x,△y)
    const int measureNum=2;                                    //测量值2×1向量(x,y)
    KalmanFilter KF(stateNum, measureNum, 0);

//    KF.transitionMatrix = *(Mat_(4, 4) <<1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1);  //转移矩阵A
    KF.transitionMatrix = (Mat_(4, 4) <<1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1);  //转移矩阵A
    setIdentity(KF.measurementMatrix);                                             //测量矩阵H
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));                            //系统噪声方差矩阵Q
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R
    setIdentity(KF.errorCovPost, Scalar::all(1));                                  //后验错误估计协方差矩阵P
    rng.fill(KF.statePost,RNG::UNIFORM,0,winHeight>winWidth?winWidth:winHeight);   //初始状态值x(0)
    Mat measurement = Mat::zeros(measureNum, 1, CV_32F);                           //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义

    namedWindow("kalman");
    setMouseCallback("kalman",mouseEvent);

    Mat image(winHeight,winWidth,CV_8UC3,Scalar(0));

    while (1)
    {
        //2.kalman prediction
        Mat prediction = KF.predict();
        Point predict_pt = Point(prediction.at(0),prediction.at(1) );   //预测值(x',y')

        //3.update measurement
        measurement.at(0) = (float)mousePosition.x;
        measurement.at(1) = (float)mousePosition.y;

        //4.update
        KF.correct(measurement);

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

        //draw
//        分别显示前一状态的位置：Point statePt = Point( (int)KF.statePost.at(0), (int)KF.statePost.at(1));;
//        预测位置：Point predictPt = Point( (int)prediction.at(0), (int)prediction.at(1));
//        观测位置(真实位置)：mousePosition（由setMouseCallback("Kalman", mouseEvent);得到，递归方式通过measurement计算得到）
        image.setTo(Scalar(255,255,255,0));
        circle(image,predict_pt,5,Scalar(0,255,0),3);    //predicted point with green
        circle(image,mousePosition,5,Scalar(255,0,0),3); //current position with red

//        char buf[256];
//        sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);
//        putText(image,buf,Point(10,30),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);
//        sprintf_s(buf,256,"current position :(%3d,%3d)",mousePosition.x,mousePosition.y);
//        putText(image,buf,cvPoint(10,60),CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,Scalar(0,0,0),1,8);

        cout<<"predicted position: "<

 

 

kalman筛选器跟踪旋转点

https://docs.opencv.org/3.4.1/d1/da2/kalman_8cpp-example.html

/** @example kalman.cpp
An example using the standard Kalman filter
*/
/** @brief Kalman filter class.

The class implements a standard Kalman filter ,
@cite Welch95 . However, you can modify transitionMatrix, controlMatrix, and measurementMatrix to get
an extended Kalman filter functionality.
@note In C API when CvKalman\* kalmanFilter structure is not needed anymore, it should be released
with cvReleaseKalman(&kalmanFilter)
 */
class CV_EXPORTS_W KalmanFilter
{
public:
    CV_WRAP KalmanFilter();
    /** @overload
    @param dynamParams Dimensionality of the state.
    @param measureParams Dimensionality of the measurement.
    @param controlParams Dimensionality of the control vector.
    @param type Type of the created matrices that should be CV_32F or CV_64F.
    */
    CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

    /** @brief Re-initializes Kalman filter. The previous content is destroyed.

    @param dynamParams Dimensionality of the state.
    @param measureParams Dimensionality of the measurement.
    @param controlParams Dimensionality of the control vector.
    @param type Type of the created matrices that should be CV_32F or CV_64F.
     */
    void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

    /** @brief Computes a predicted state.

    @param control The optional input control
     */
    CV_WRAP const Mat& predict( const Mat& control = Mat() );

    /** @brief Updates the predicted state from the measurement.

    @param measurement The measured system parameters
     */
    CV_WRAP const Mat& correct( const Mat& measurement );

    CV_PROP_RW Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)
    CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
    CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)
    CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)
    CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
    CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
    CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

实验部分

　　本次实验来源于opencv自带sample中的例子，该例子是用kalman来完成一个一维的跟踪，即跟踪一个不断变化的角度。在界面中表现为一个点在圆周上匀速跑，然后跟踪该点。看起来跟踪点是个二维的，其实转换成角度就是一维的了。

C调用opencv的kalman筛选器的示例。\n“
“跟踪旋转点。\n”
“旋转速度恒定。\n”
“状态和测量向量都是1d（点角），\n”
“测量是实际点角度+高斯噪声。\n”
“实际点和估计点与黄线段连接，”\n“
“实际点和测量点与红线段相连。\n”
（如果卡尔曼滤波器工作正常，则为\n）
“黄色段应短于红色段）。\n”
“按任意键（Esc除外）将以不同的速度重置跟踪。\n”
“按Esc将停止程序

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include 
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( "\nExample 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);
    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;
}

• 
• phi希腊字母表的第21个字母

　　该代码中,有这么几句
　　KalmanFilter KF(2, 1, 0);
　　...
　　KF.transitionMatrix = *(Mat_(2, 2) << 1, 1, 0, 1);
　　...

　　Mat_(2, 2) << 1, 1, 0, 1是一个整体,即往Mat_(2, 2)的矩阵中赋值1,1,0,1;说白了就是给Mat矩阵赋初值，因为初值没什么规律，所以我们不能用zeros，ones等手段来赋值。比如运行下面语句时：

　　Mat a;

　　a = (Mat_(2, 2) << 1, 1, 0, 1);

 　　cout<

 　 其结果就为

　　[1,1;

　　0,1]

测量维数定义问题：
1 你可以替换代码便于理解

KalmanFilter KF(2, 2, 0);// KalmanFilter KF(2, 2, 0); //创建卡尔曼滤波器对象KF  //状态维数2，测量维数1，没有控制量
Mat state(2, 1, CV_32F); /* (phi, delta_phi) */   //state(角度，△角度)  //状态值，（角度，角速度）
Mat processNoise(2, 1, CV_32F);//过程噪声 Q
Mat measurement =  (Mat_(2, 1) << 0, 0);//Mat::zeros(1, 1, CV_32F);//定义测量值

2  你可以留意代码便于理解  输出measurement.at(1)

// generate measurement           //measurement是测量值
measurement += KF.measurementMatrix*state;//z = z + H*x;
double measAngle = measurement.at(0);

所以  KalmanFilter KF(2, 1, 0);有自取所需功能（个人感觉是）。

#include 
#include "opencv2/video/tracking.hpp"
//#include     //#include "cvAux.h"
#include 
#include 
#include 

using namespace cv;
using namespace std;

int main( )
{
    float A[10][3] =
            {
                    10,    50,     15.6,
                    12,    49,     16,
                    11,    52,     15.8,
                    13,    52.2,   15.8,
                    12.9,  50,     17,
                    14,    48,     16.6,
                    13.7,  49,     16.5,
                    13.6,  47.8,   16.4,
                    12.3,  46,     15.9,
                    13.1,  45,     16.2
            };

    const int stateNum=3;
    const int measureNum=3;
    KalmanFilter KF(stateNum, measureNum, 0);
    KF.transitionMatrix = (Mat_(3, 3) <<1,0,0,0,1,0,0,0,1);  //转移矩阵A     //*(Mat_(3, 3) <<1,0,0,0,1,0,0,0,1);  //转移矩阵A
    setIdentity(KF.measurementMatrix);                                             //测量矩阵H
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));                            //系统噪声方差矩阵Q
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R
    setIdentity(KF.errorCovPost, Scalar::all(1));
    Mat measurement = Mat::zeros(measureNum, 1, CV_32F);

    //初始状态值
    KF.statePost = (Mat_(3, 1) <(3, 1) <(0)<<"\t"<(1)<<"\t"<(2)<(0)<<"\t"<(1)<<"\t"<(2)<(0)<<"\t"<(1)<<"\t"<(2)<