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;}