卡尔曼（Kalman） 滤波跟踪一个旋转的点程序

学习了一下函数：

1）CvKalman的结构

2）CvRandState的结构

3）cvRandSetRange()

4）cvRandInit()

5）memcpy(void *dest, void *src, unsigned int count)

6）cvSetIdentity()

7）cvMatMulAdd(src1,src2,src3,dst)

#include "cv.h"

#include "highgui.h"
#include <math.h>
int main(int argc, char** argv)
{
    /* A matrix data */
    const float A[] = { 1, 1, 0, 1 };
    IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
    CvKalman* kalman = cvCreateKalman( 2, 1, 0 );
    /* state is (phi, delta_phi) - angle and angle increment */
    CvMat* state = cvCreateMat( 2, 1, CV_32FC1 );
    CvMat* process_noise = cvCreateMat( 2, 1, CV_32FC1 );
    /* only phi (angle) is measured */
    CvMat* measurement = cvCreateMat( 1, 1, CV_32FC1 );
    CvRandState rng;
    int code = -1;
    cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
    cvZero( measurement );
    cvNamedWindow( "Kalman", 1 );
    for(;;)
    {
        cvRandSetRange( &rng, 0, 0.1, 0 );
        rng.disttype = CV_RAND_NORMAL;
        cvRand( &rng, state );
        memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
        cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );//初始化带尺度的单位矩阵 
        cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );
        cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );
        cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));
        /* choose random initial state */
        cvRand( &rng, kalman->state_post );
        rng.disttype = CV_RAND_NORMAL;
        for(;;)
        {
            #define calc_point(angle)                                      \
                cvPoint( cvRound(img->width/2 + img->width/3*cos(angle)),  \
                         cvRound(img->height/2 - img->width/3*sin(angle)))
            float state_angle = state->data.fl[0];
            CvPoint state_pt = calc_point(state_angle);
            /* predict point position */
            const CvMat* prediction = cvKalmanPredict( kalman, 0 );
            float predict_angle = prediction->data.fl[0];
            CvPoint predict_pt = calc_point(predict_angle);
            float measurement_angle;
            CvPoint measurement_pt;

            cvRandSetRange( &rng, 0, sqrt(kalman->measurement_noise_cov->data.fl[0]), 0 );
            cvRand( &rng, measurement );
            /* generate measurement */
            cvMatMulAdd( kalman->measurement_matrix, state, measurement, measurement );
            measurement_angle = measurement->data.fl[0];
            measurement_pt = calc_point(measurement_angle);

            /* plot points */
            #define draw_cross( center, color, d )                                 \
                cvLine( img, cvPoint( center.x - d, center.y - d ),                \
                             cvPoint( center.x + d, center.y + d ), color, 1, 0 ); \
                cvLine( img, cvPoint( center.x + d, center.y - d ),                \
                             cvPoint( center.x - d, center.y + d ), color, 1, 0 )
            cvZero( img );
            draw_cross( state_pt, CV_RGB(255,255,255), 3 );
            draw_cross( measurement_pt, CV_RGB(255,0,0), 3 );
            draw_cross( predict_pt, CV_RGB(0,255,0), 3 );
            cvLine( img, state_pt, predict_pt, CV_RGB(255,255,0), 3, 0 );

            /* adjust Kalman filter state */
            cvKalmanCorrect( kalman, measurement );
            cvRandSetRange( &rng, 0, sqrt(kalman->process_noise_cov->data.fl[0]), 0 );
            cvRand( &rng, process_noise );
            cvMatMulAdd( kalman->transition_matrix, state, process_noise, state );

            cvShowImage( "Kalman", img );
            code = cvWaitKey( 100 );
            if( code > 0 ) /* break current simulation by pressing a key */
                break;
        }
        if( code == 27 ) /* exit by ESCAPE */
            break;
    }
    return 0;

}

这是一个比较有深度的程序，具体解析还无法说明。不过有很多地方可以学习。

1）Kalman 滤波器状态

typedef struct CvKalman
{ int MP; /* 测量向量维数 */
int DP; /* 状态向量维数 */
int CP; /* 控制向量维数 */
/* 向后兼容字段 */#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; /* 预测状态 (x'(k)): x(k)=A*x(k-1)+B*u(k) */
CvMat* state_post; /* 矫正状态 (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) (如果没有控制，则不使用它)*/
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;

结构 CvKalman 用来保存 Kalman 滤波器状态。它由函数 cvCreateKalman 创建，由函数cvKalmanPredict 和 cvKalmanCorrect 更新，由 cvReleaseKalman 释放. 通常该结构是为标准 Kalman 所使用的:
其中:
对标准 Kalman 滤波器，所有矩阵: A, B, H, Q 和 R 都是通过 cvCreateKalman 在分配结构 CvKalman 时初始化一次。但是，同样的结构和函数，通过在当前系统状态邻域中线性化扩展 Kalman 滤波器方程，可以用来模拟扩展 Kalman 滤波器，在这种情况下， A, B, H (也许还有 Q 和 R) 在每一步中都被更新。

2）首先，CvRandState是opencv提供的另一个生成随机数的结构体，它包含在“cvcompat.h”中。其中， "cvcompat.h"的完整意思是Computer Vision Compatility。另外，对于CvRandState来说，它的特色在于可以选定常态分布和均匀分布，但它只能应用于矩阵和一维阵列。
 最后，给出CvRandState的结构如下：
  typedef struct CvRandState 
{ 
CvRNG state;  
int disttype;  
CvScalar param[2];  
} 
CvRandState; 

typedef uint64 CvRNG; 

typedef unsigned __int64 uint64; 

typedef struct CvScalar 
{ 
double val[4]; 
} 
CvScalar;

然后，需要补充一些更重要的东西。因为常常用到随机数初始化，是用函数cvRandInt()，但对于CvRandState则用cvRandInit()来初始化。
函数cvRandInit()的用法如下：
用法1：
cvRandInit(CvRandState结构,随机上界,随机下界,均匀分布参数,64bits种子的数字);
用法2：
cvRandInit(CvRandState结构,平均数,标准差,常态分布参数,64bits种子的数字);

3）cvRandSetRange()
修改CvRandState数据结构的参数内容,均匀分布的话可以每个信道的上下界常态分布可以修改每个通道的平均数,标准偏差.
cvRandSetRange(CvRandState数据结构,均匀分布上界,均匀分布下界,目标信道数据)
cvRandSetRange(CvRandState数据结构,常态分布平均数,常态分布标准偏差,目标信道数据)
其中CvRandState产生随机向量的结构数组，cvRandInit对CvRandState的初始化。
cvRandInit( &rng_state,0, 1, 0xffffffff , CV_RAND_UNI )
初始化CvRandState资料结构,可以选定随机分布的种类,并给他定种子,有两种情形
cvRandInit(CvRandState资料结构,随机上界,随机下界,均匀分布的参数,指定分布类型（CV_RAND_UNI）)
cvRandInit(CvRandState资料结构,平均数,标准差,正态分布参数,正态分布类型（CV_RAND_NORMAL）)
如：
CvRandState rng;
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
同时cvRandSetRange可以在不重新初始化rng状态（CV_RAND_UNI/CV_RAND_NORMAL）的情况下改变已经初始化的rng范围及相关参数。
例如：
cvRandSetRange( &rng, 0, 0.1, 0 );
rng.disttype = CV_RAND_NORMAL;//上边设置rng为CV_RAND_UNI状态，现在改为CV_RAND_NORMAL

4）原型：extern void *memcpy(void *dest, void *src, unsigned int count);
用法：#include <string.h>
功能：由src所指内存区域复制count个字节到dest所指内存区域。

5）cvSetIdentity()将矩阵行与列相等的元素置为1，其余元素置为0

6）cvMatMulAdd(src1,src2,src3,dst)
功能是：dist=src1*src2+src3
src1 第一输入数组
src2 第二输入数组
src3 第三输入数组
dst  输出数组