Opencv的Kalman滤波器函数二

Opencv提供了4个与应用Kalman滤波器直接相关的函数
一、四个函数
1、创建Kalman数据结构
cvCreateKalman(int nDynamParams,
int nMeasureParams,
int nControlParams);
2、返回（删除）Kalman数据结构
cvReleaseKalman(CvKalmen** kalman);
3、计算下一个时间点的预期值
cvKalmanPredict(CvKalman* Kalman,
const CvMat* control = NULL);
4、校正新的测量值
cvKalmanCorrect(CvKalman* kalman,
CvMat* measured ;)
二、CvKalman 数据结构

typedef struct CvKalman
{
    int MP;                     /* number of measurement vector dimensions */
    int DP;                     /* number of state vector dimensions */
    int CP;                     /* number of control vector dimensions */

    /* backward compatibility fields */
#if 1
    float* PosterState;         /* =state_pre->data.fl */
    float* PriorState;          /* =state_post->data.fl */
    float* DynamMatr;           /* =transition_matrix->data.fl */
    float* MeasurementMatr;     /* =measurement_matrix->data.fl */
    float* MNCovariance;        /* =measurement_noise_cov->data.fl */
    float* PNCovariance;        /* =process_noise_cov->data.fl */
    float* KalmGainMatr;        /* =gain->data.fl */
    float* PriorErrorCovariance;/* =error_cov_pre->data.fl */
    float* PosterErrorCovariance;/* =error_cov_post->data.fl */
    float* Temp1;               /* temp1->data.fl */
    float* Temp2;               /* temp2->data.fl */
#endif

    CvMat* state_pre;           /* predicted state (x'(k)):
                                    x(k)=A*x(k-1)+B*u(k) */
    CvMat* state_post;          /* corrected state (x(k)):
                                    x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
    CvMat* transition_matrix;   /* state transition matrix (A) */
    CvMat* control_matrix;      /* control matrix (B)
                                   (it is not used if there is no control)*/
    CvMat* measurement_matrix;  /* measurement matrix (H) */
    CvMat* process_noise_cov;   /* process noise covariance matrix (Q) */
    CvMat* measurement_noise_cov; /* measurement noise covariance matrix (R) */
    CvMat* error_cov_pre;       /* priori error estimate covariance matrix (P'(k)):
                                    P'(k)=A*P(k-1)*At + Q)*/
    CvMat* gain;                /* Kalman gain matrix (K(k)):
                                    K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
    CvMat* error_cov_post;      /* posteriori error estimate covariance matrix (P(k)):
                                    P(k)=(I-K(k)*H)*P'(k) */
    CvMat* temp1;               /* temporary matrices */
    CvMat* temp2;
    CvMat* temp3;
    CvMat* temp4;
    CvMat* temp5;
}
CvKalman;

三、简单程序实现及解析

// 包含头文件
#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
#include "ml.h"

// 宏定义
// 符号，表示mat的圆心坐标cvPoint(x, y)
#define phi2xy(mat) cvPoint(cvRound(img->width/2 + img->width/3 * cos(mat->data.fl[0])),cvRound(img->height / 2 - img->width/3 * sin(mat->data.fl[0])))


int main(void)
{

    //////////////////////////////////////////////////////////////////////////
    //初始化各个参数和矩阵:
    //1、状态值x_k,  2、过程噪声矩阵w_k,  3、测量值z_k(清0)， 4、转换矩阵：kalman->transition_matrix->data.fl（F）
    //5、测量矩阵H， 6、过程噪声协方差，  7、测量噪声协方差， 8、后验误差协方差
    //////////////////////////////////////////////////////////////////////////
    // 创建窗口
    cvNamedWindow("Kalman", 1);

    //  创建一个随机数产生器
    CvRandState rng;
    cvRandInit(&rng, 0, 1, -1, CV_RAND_UNI);

    // 一幅用于绘制的图像
    IplImage* img = cvCreateImage(cvSize(500, 500), 8, 3);  // 图像大小

    // 创建卡尔曼滤波器结构
    CvKalman* kalman = cvCreateKalman(2, 1, 0); // 2(速度、角速度)-状态量， 1(只能测量车辆的位置)-测量量, 0(无控制量)

    // 用随机数初始化状态
    CvMat* x_k = cvCreateMat(2, 1, CV_32FC1);   // 创建矩阵:状态x_k
    cvRandSetRange(&rng, 0, 0.1, 0);
    rng.disttype = CV_RAND_NORMAL;
    cvRand(&rng, x_k);      // 初始化x_k,为分布在0附近的合理的随机数

    // 过程噪声
    CvMat* w_k = cvCreateMat(2, 1, CV_32FC1);   // 创建过程噪声矩阵

    // 测量量，只有一个：位置
    CvMat* z_k = cvCreateMat(1, 1, CV_32FC1);   // 创建测量值的矩阵
    cvZero(z_k);    // 清0

    // 转换矩阵，描述模型的k和k+1步的参数
    const float F[4] = {1, 1, 0, 1};    // 2*2矩阵
    memcpy(kalman->transition_matrix->data.fl, F, sizeof(F));   // 将矩阵F赋值给kalman滤波器的转换矩阵（CvKalman结构中的transition_matrix）


    // 初始化卡尔曼滤波的其他参数
    cvSetIdentity(kalman->measurement_matrix, cvRealScalar(1)); // 测量矩阵H
    cvSetIdentity(kalman->process_noise_cov, cvRealScalar(1e-5));   // 过程噪声协方差
    cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(1e-1));   // 测量噪声协方差
    cvSetIdentity(kalman->error_cov_post, cvRealScalar(1));     // 后验误差协方差
    // cvRealScalar（）把val四个量的第一个val[0]置为想要设的值，其他三个为0

    // 选择随机数初始状态
    cvRand(&rng, kalman->state_post);

    while(1)
    {
        //////////////////////////////////////////////////////////////////////////
        // 开始预测
        //////////////////////////////////////////////////////////////////////////


        // 1、首先，用kalman滤波器预测它所认为在此阶段会产生声明（也就是在获得任何新的信息之前）---预测值：y_k
        // 预测点的位置
        const CvMat* y_k = cvKalmanPredict(kalman, 0);  // 控制量为0


        ///2、然后，为这次迭代产生测量值的新值---测量值:z_k
        // 产生测量值
        cvRandSetRange(&rng, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0);
        cvRand(&rng, z_k);
        cvMatMulAdd(kalman->measurement_matrix, x_k, z_k, z_k); // z_k（第四个参数）是x_k*H(measurement_matrix)+测量噪声，此处的测量噪声（第三个参数）为随机数z_k

        // 画出先前合成的观测值，测量、预测、实际状态
        cvZero(img);
        cvCircle(img, phi2xy(z_k), 4, CV_RGB(255, 255, 0)); // 测量值
        cvCircle(img, phi2xy(y_k), 4, CV_RGB(255, 255, 255));   // 预测值
        cvCircle(img, phi2xy(x_k), 4, CV_RGB(255, 0, 0));   // 实际值

        // 3、至此，可以开始下一次迭代
        // 校正新的测量值
        cvKalmanCorrect(kalman, z_k);   // 赋予卡尔曼滤波器最新的测量值


        // 4、产生过程噪声，然后产生新的实际值，可以开始新一轮的计算
        cvRandSetRange(&rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0);
        cvRand(&rng, w_k);
        cvMatMulAdd(kalman->transition_matrix, x_k, w_k, x_k);  // x_k = x_k-1  * transition_matrix(F)   +  w_k; 

        cvShowImage("Kalman", img);
        // 按"Esc"键退出程序
        if (cvWaitKey(100) == 27)
        {
            break;
        }

    }
    return 0;
}