卡尔曼滤波(Kalman filtering)小结

最新推荐文章于 2024-04-23 22:39:42 发布

通信程序猿

最新推荐文章于 2024-04-23 22:39:42 发布

阅读量2.6w

点赞数 7

分类专栏： OpenCV 图像处理文章标签：算法应用

本文链接：https://blog.csdn.net/u011285477/article/details/52070900

版权

OpenCV 同时被 2 个专栏收录

15 篇文章 1 订阅

订阅专栏

图像处理

8 篇文章 0 订阅

订阅专栏

最近项目用到了kalman滤波，本博文简单介绍下卡尔曼滤波器的概念、原理和应用，做个小结。

概念

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表[1]。

卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题[2]。

应用

例如,对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。
扩展卡尔曼滤波（EXTEND KALMAN FILTER, EKF）是由kalman filter考虑时间非线性的动态系统，常应用于目标跟踪系统。

kalman滤波器

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

在kalman滤波器滤波器应用中，一般考虑三种运动：
1）动态运动：这种运动是我们期望的前次测量时系统状态的直接结果，如匀速运动等。
2）控制运动：这种运动是我们期望的，由于某种已知的外部因素以某种原因施加于系统，如加速运动等。
3）随机运动：随机的无规则运动，在Kalman滤波器中，至少是可以用高斯模型有效地来建模。

以下是两个重要方程：
状态方程：

x k = A x k - 1 + B u k - 1 + q k - 1

$x_k=Ax_{k-1}+Bu_{k-1}+q_{k-1}$
观测方程：

y k = H x k + r k

$y_k=Hx_k+r_k$
上两式子中，x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。A是传输参数、B是控制参数，都是系统参数，对于多模型系统，他们为矩阵。y(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为状态转移矩阵。q(k)和r(k)分别表示状态和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

KF算法流程如下图：

这里的K是kalman增益矩阵。

五个重要更新公式：
时间更新：

x^- k = A x^k - 1 + B u k - 1 (1)

$\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1}\ (1)$
上标”-“表示“在新的测量之前”，求出先验估计

x^−k $\hat{x}_k^-$ ;

P - k = A P k - 1 A T + Q (2)

$P_k^-=AP_{k-1}A^T+Q\ (2)$
这个等式构成预测部分的基础，这告诉我们根据已看到的“我们可以期望什么”;
状态更新：

K k = P - k H T (H P - k H T + R) - 1 (3)

$K_k=P_k^-H^T(HP_k^-H^T+R)^{-1}\ (3)$
这里给出所谓的Kalman增益或更新率或混合比例，告诉我们如何给定新信息相对已知信息的权重，在直接测量一个位置变量的一维例子中，状态误差是

σ2k−1 $\sigma_{k-1}^2$ ，测量误差是

σ2k $\sigma_k^2$ ，

K=σ2k−1σ2k−1+σ2k $K=\frac{\sigma_{k-1}^2}{\sigma_{k-1}^2+\sigma_{k}^2}$ ；

x^k = x^- k + K k (y k - H x^- k) (4)

$\hat{x}_k=\hat{x}_k^-+K_k(y_k-H\hat{x}_k^-)\ (4)$

P k = (I - K k H) P - k (5)

$P_k=(I-K_kH)P_k^-\ (5)$
得到“新的测量之后”，就可以用更新率K对

x^−k、P−k $\hat{x}_k^-、P_k^-$ 做更新得到最优的更新值

x^k、Pk $\hat{x}_k、P_k$ 。

举例

以测量在停车场行驶的汽车为实际例子，汽车的状态用两个位置变量 $x$ 和 $y$ ，以及两个速度变量 $v_x$ 和 $v_y$ 表示。这四个变量组成状态向量 $x_k$ 的元素。 $dt$ 表示速度增量。这里假设的是动态运动模型。

x k = [x y v x v y] T

$x_k = [x\ y\ v_x\ v_y]^T$

A = ⎡ ⎣ ⎢ ⎢ ⎢ 10000100 d t 010 0 d t 01 ⎤ ⎦ ⎥ ⎥ ⎥

$\begin{gather*} A=\begin{bmatrix} 1&0&dt&0 \\ 0&1&0&dt\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} \end{gather*}$
当使用摄像机观测汽车的状态时，可能只观测量到位置变量：

y k = [y x y y]

$\begin{gather*} y_k=\begin{bmatrix} y_x \\ y_y\\ \end{bmatrix} \end{gather*}$

状态转移矩阵H结构如下：

H = ⎡ ⎣ ⎢ ⎢ ⎢ 10000100 ⎤ ⎦ ⎥ ⎥ ⎥

$\begin{gather*} H=\begin{bmatrix} 1&0\\ 0&1\\ 0&0\\ 0&0\\ \end{bmatrix} \end{gather*}$
在该例子中，可能并不真正认为汽车速度不变，所以要设置

Q $Q$ 来反应这个问题。对视频流用图像分析方法来测量汽车的位置，再根据对测量精确度的估计来选择

R $R$ 。
这些量初始化完成后，嵌入到kalman滤波的那些公式中，便可以进行汽车位置估计。

在项目中，用kalman跟踪物体质心运动，建模思路跟这个汽车例子类似，因为运动不是匀速存在偏差（状态误差），因为检测器检测到的目标的质心的位置存在偏差（观测误差），而这些偏差和设定会影响到kalman的跟踪效果。

OpenCV demo

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <stdio.h>

using namespace cv;

static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}

static void help()
{
    printf( "\nExamle of c calls to OpenCV's Kalman filter.\n"
"   Tracking of rotating point.\n"
"   Rotation speed is constant.\n"
"   Both state and measurements vectors are 1D (a point angle),\n"
"   Measurement is the real point angle + gaussian noise.\n"
"   The real and the estimated points are connected with yellow line segment,\n"
"   the real and the measured points are connected with red line segment.\n"
"   (if Kalman filter works correctly,\n"
"    the yellow segment should be shorter than the red one).\n"
            "\n"
"   Pressing any key (except ESC) will reset the tracking with a different speed.\n"
"   Pressing ESC will stop the program.\n"
            );
}

int main(int, char**)
{
    help();
    Mat img(500, 500, CV_8UC3);
    KalmanFilter KF(2, 1, 0);
    Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
    Mat processNoise(2, 1, CV_32F);
    Mat measurement = Mat::zeros(1, 1, CV_32F);
    char code = (char)-1;

    for(;;)
    {
        randn( state, Scalar::all(0), Scalar::all(0.1) );
        KF.transitionMatrix = *(Mat_<float>(2, 2) << 1, 1, 0, 1);

        setIdentity(KF.measurementMatrix);
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(KF.errorCovPost, Scalar::all(1));

        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));

        for(;;)
        {
            Point2f center(img.cols*0.5f, img.rows*0.5f);
            float R = img.cols/3.f;
            double stateAngle = state.at<float>(0);
            Point statePt = calcPoint(center, R, stateAngle);

            Mat prediction = KF.predict();
            double predictAngle = prediction.at<float>(0);
            Point predictPt = calcPoint(center, R, predictAngle);

            randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

            // generate measurement
            measurement += KF.measurementMatrix*state;

            double measAngle = measurement.at<float>(0);
            Point measPt = calcPoint(center, R, measAngle);

            // plot points
            #define drawCross( center, color, d )                                 \
                line( img, Point( center.x - d, center.y - d ),                \
                             Point( center.x + d, center.y + d ), color, 1, CV_AA, 0); \
                line( img, Point( center.x + d, center.y - d ),                \
                             Point( center.x - d, center.y + d ), color, 1, CV_AA, 0 )

            img = Scalar::all(0);
            drawCross( statePt, Scalar(255,255,255), 3 );
            drawCross( measPt, Scalar(0,0,255), 3 );
            drawCross( predictPt, Scalar(0,255,0), 3 );
            line( img, statePt, measPt, Scalar(0,0,255), 3, CV_AA, 0 );
            line( img, statePt, predictPt, Scalar(0,255,255), 3, CV_AA, 0 );

            if(theRNG().uniform(0,4) != 0)
                KF.correct(measurement);

            randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
            state = KF.transitionMatrix*state + processNoise;

            imshow( "Kalman", img );
            code = (char)waitKey(100);

            if( code > 0 )
                break;
        }
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }

    return 0;
}

运行效果：

原文链接：http://blog.csdn.net/u011285477/article/details/52070900

参考资料：
[1]卡尔曼滤波-百度百科
[2]对Kalman(卡尔曼)滤波器的理解-CSDN博客
[3]Kalman滤波器从原理到实现-CSDN博客
[4]《学习OpenCV（中文版）》于仕琪、刘瑞祯译清华大学出版社 P384

通信程序猿

关注

7
点赞
踩
66

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波(Kalman filtering)小结

最近项目用到了kalman滤波，本博文简单介绍下卡尔曼滤波器的概念、原理和应用，做个小结。概念卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
复制链接

扫一扫