OpenCV卡尔曼滤波学习

目录

前言

终究逃不过卡尔曼滤波，讲道理今年这一年听到好多次卡尔曼滤波，然后也断断续续学习了卡尔曼滤波。如今再次需要用到卡尔曼滤波，希望在年前能完全理解之。

卡尔曼滤波器原理

图像滤波概述

图像滤波，指在尽量保留图像细节特征的条件下对目标图像的噪声进行抑制，是图像预处理中不可缺少的操作，其处理效果的好坏将直接影响到后续图像处理和分析的有效性和可靠性。

消除图像中的噪声成分叫做图像的平滑化或滤波操作。信号或图像的能量大部分集中在幅度谱的低频和中频段，而在较高频段，有用的信息经常被噪声淹没。因此一个能降低高频成分幅度的滤波器就能够减弱噪声的影响。

图像滤波的目的是两个：一个是抽出对象的特征作为图像识别的特征模式(比如：高通滤波获取对象的边缘特征)；另一个是为适应图像处理的要求，消除图像数字化所混入的噪声(低通滤波)。
而对滤波处理的要求也有两条：一是不能损坏图像的轮廓及边缘等重要信息；二是使图像清晰视觉效果好。

关于滤波器，一种形象的比喻是：可以把滤波器想象成一个包含加权系数的窗口，当使用这个滤波器平滑处理图像时，就把这个窗口放到图像之上，透过这个窗口来看我们得到的图像。

图像滤波原理

图像本质上就是各种色彩波的叠加。图像就是色彩的波动：波动大，就是色彩急剧变化；波动小，就是色彩平滑过渡。因此，波的各种指标可以用来描述图像。

频率是波动快慢的指标，单位时间内波动次数越多，频率越高，反之越低。

色彩剧烈变化的地方，就是图像的高频区域；色彩稳定平滑的地方，就是低频区域。

低通滤波器使得图像的高频区域变成低频，即色彩变化剧烈的区域变得平滑，也就是出现模糊效果。

高通滤波器过滤低频，只保留那些变化最快速最剧烈的区域，也就是图像里面的物体边缘，常用于边缘识别。

滤波是将信号中特定波段频率滤除的操作，是抑制和防止干扰的一项重要措施。高斯滤波是指用高斯函数作为滤波函数的滤波操作，高斯低通滤波就是滤除高频率，这样相当于把比较明显的特征模糊。高斯高通滤波就是锐化。

线性滤波器

线性滤波器经常用于剔除输入信号中不想要的频率或者从许多频率中选择一个想要的频率。几种常见的线性滤波器如下：

• 低通滤波器：运行低频率通过
• 高通滤波器：运行高频率通过
• 带通滤波器：允许一定范围频率通过
• 带阻滤波器：阻止一定范围频率通过并且允许其他频率通过
• 全通滤波器：允许所有频率通过，仅仅改变相位关系
• 陷波滤波器：阻止一个狭窄频率范围通过，是一种特殊带阻滤波器
• 卡尔曼滤波器：最佳的线性滤波器 描述状态与状态之间的线性关系

卡尔曼滤波

你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
在连续变化的系统中，使用卡尔曼滤波是非常理想的，它具有占用内存小的优点，并且速度很快，很适合用于实时问题和嵌入式系统。

卡尔曼滤波是建立在动态过程之上，由于物理量(位移、速度)的不可突变特性，可以通过t-1时刻估计(预测)t时刻的状态。
1. $预测值(x'_k): x'_k=A*x_{k-1}+B*u_k$
2. $最小均方误差(P'_k): P'_k=A*P_{k-1}*A^T + Q)$
其中：
$x'_k$是预测状态
$x_{k-1}$是前一步的校正状态，应该在开始的某个地方初始化，即缺省为零向量。
$u_k$是外部控制参数
$P'_k$是先验误差相关矩阵
$P_{k-1}$是前一步的后验误差矩阵，应该在开始的某个地方初始化，即缺省为单位矩阵。
3. $卡尔曼增益(K_k): K_k=P'_k*H^T*(H*P'_k*H^T+R)^{-1}$
权衡$预测协方差P$和$观测协方差R$的大小，来决定我们是相信预测模型还是相信观测模型。
4. $状态值(x_k): x_k=x'(k)+K_k*(z_k-H*x'_k)$
$Z_k$是当前时刻k的测量状态，通过在测量的估计值和观测值之间取权重值，得到状态值。$H$是观测矩阵。$R$是观测噪声协方差
5. $修正的最小均方误差(P(k)): P_k=(I-K_k*H)*P'_k$
噪声(状态)协方差矩阵的更新，也就是根据当前时刻预测协方差获得用于下一个时刻的协方差。

两个维度的不确定性，我们用方差矩阵无法得知这两个维度之间的相关性。(方差就是中心值+方差，这样可以表示不确定性)，但是对于两个维度的相关性，也就是描述两个维度的方差的特性的时候，需要使用协方差矩阵。当协方差为0，表示两个维度不相关；当协方差大于0，表示正相关(也就是一个维度增大，另一个维度也增大)；当协方差小于0，表示负相关(也就是一个维度增大，另一个维度减小)。

噪声协方差矩阵的传递：由上一时刻的协方差推测当前时刻的协方差，要乘上状态转移矩阵和状态转移矩阵的转置。$P'_k=AP_{k-1}A^T$。协方差矩阵的性质：$cov(Ax,Bx)=Acov(x,x)B^T$。但是协方差矩阵的传递过程也是有噪声的，所以用Q**状态转移协方差矩阵表示这个模型所带来的噪声。$P'_k=AP_{k-1}A^T+Q$[表示不确定性在状态之间的传递关系**]

观测矩阵：观测得来的值也是具有不确定性的，也就是存在观测噪声。所以也就是存在观测噪声的协方差矩阵。

OpenCV1.0卡尔曼滤波

• CvKalman

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释放。

$系统运动方程: x'_k=A*x_{k-1}+B*u_k+w_k$
$系统观测方程:z_k=H*x_k+v_k$
其中：
A是状态转移矩阵
$x_{k-1}$是系统在$k(k-1)$的状态向量
B是控制矩阵
$u_k$应用于时刻$k$的外部控制向量
$z_k$是在时刻$k$的系统状态测量向量
H是测量矩阵
$w_k$和$v_k$分别为正态分布的运动和测量噪声，都是高斯噪声，符合正态分布。

• cvCreateKalman

CvKalman* cvCreateKalman( int dynam_params, int measure_params, int control_params=0 );

dynam_params：状态向量维数
measure_params：测量向量维数
control_params：控制向量维数
函数cvCreateKalman分配CvKalman以及它的所有矩阵和初始参数。

• KalmanPredict

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

kalman：Kalman滤波器状态
control：控制向量$u_k$，如果没有外部控制应该为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),应该在开始的某个地方初始化，即缺省为单位矩阵.
功能：函数返回估计得到的状态值
状态更新：在预测值和估计值之间取最优估计值。

• KalmanCorrect

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

kalman：被更新的Kalman结构指针
measurement：指向测量向量的指针，向量形式为CvMat
函数cvKalmanCorrect在给定模型的状态的测量基础上，调节随机模型状态：
$K_k=P'_k*H^T*(H*P'_k*H^T+R)^{-1}$
$x_k=x'(k)+K_k*(z_k-H*x'_k)$
$P_k=(I-K_k*H)*P'_k$
其中：
$z_k$是给定测量值
$K_k$是Kalman“增益矩阵”
函数存储调节状态到kalman->state_post中并且输出时返回它。

OpenCV2.0卡尔曼滤波

参考链接

《OpenCV3编程入门_毛星云编著》
阮一峰：图像与滤波
Cv运动分析与对象跟踪
卡尔曼滤波器（Kalman Filtering）入门
Kalman滤波器从原理到实现
详解卡尔曼滤波原理
卡尔曼滤波器的原理以及在matlab中的实现