OpenCV Kalman滤波器_undefined reference to cv::kalmanfilter-CSDN博客

本文链接：https://blog.csdn.net/tiemaxiaosu/article/details/51518126

一、概述

Kalman 滤波器的基本思想是，若有一组强而合理的假设，给出系统的历时测量值，则可以建立最大化这些早前测量的后验概率的系统状态模型。另外，无需存储很长的早前测量历史，我们也可以最大化后验概率，即重复更新系统状态模型。

Kalman 滤波器需要三个重要的假设：（1）被建模的系统是线性的；（2）影响测量的噪声属于白噪声；（3）噪声本质上是高斯分布的。第一条假设的意思是 k 时刻的系统状态可以用某个矩阵与 k-1 时刻的系统状态的乘积表示。余下两条假设，即假设噪声是高斯分布的白噪声，其含义为噪声与时间不相关，且只用均值和协方差就可以准确地为幅值建模。

二、Kalman 数据结构体

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;

三、相关函数

1、分配 Kalman 滤波器结构

CvKalman* cvCreateKalman( int dynam_params, int measure_params, int control_params=0 );
dynam_params
     状态向量维数
measure_params
     测量向量维数
control_params
     控制向量维数
函数 cvCreateKalman 分配 CvKalman 以及它的所有矩阵和初始参数

2、释放 Kalman 滤波器结构

void cvReleaseKalman( CvKalman** kalman );
kalman
指向 Kalman 滤波器结构的双指针
函数 cvReleaseKalman 释放结构 CvKalman 和里面所有矩阵

3、估计后来的模型状态

const CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control=NULL );
#define cvKalmanUpdateByTime cvKalmanPredict

kalman

Kalman 滤波器状态

control

控制向量 (uk), 如果没有外部控制 (control_params=0) 应该为 NULL

函数 cvKalmanPredict 根据当前状态估计后来的随机模型状态，并存储于 kalman->state_pre:

其中

$x' k$ 是预测状态 (kalman->state_pre),

$x k ? 1$ 是前一步的矫正状态 (kalman->state_post)，应该在开始的某个地方初始化，即缺省为零向量，

$u k$ 是外部控制(control 参数),

P' k

是先验误差相关矩阵 (kalman->error_cov_pre)

P k ? 1

是前一步的后验误差相关矩阵(kalman->error_cov_post),应该在开始的某个地方初始化，即缺省为单位矩阵.

函数返回估计得到的状态值

4、调节模型状态

const CvMat* cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement );
#define cvKalmanUpdateByMeasurement cvKalmanCorrect

kalman

被更新的 Kalman 结构的指针

measurement

指向测量向量的指针，向量形式为 CvMat

函数 cvKalmanCorrect 在给定的模型状态的测量基础上，调节随机模型状态：

其中

$z k$ - 给定测量(mesurement parameter)

$K k$ - Kalman "增益" 矩阵

函数存储调节状态到 kalman->state_post 中并且输出时返回它。