为您推荐:
精华内容
最热下载
问答
  • 7.66MB u013539952 2018-04-07 16:54:02
  • 999KB qq_24489251 2019-11-06 21:59:04
  • 7KB weixin_42131367 2021-04-02 03:30:56
  • 1KB weixin_42759805 2019-12-25 20:32:18
  • 2KB qq_36800378 2019-06-29 16:32:47
  • 7KB qq_27725591 2016-10-23 14:20:59
  • 2KB l2014010671 2020-03-31 10:26:02
  • 2.54MB mrwang666 2019-03-14 22:49:07
  • 865KB weixin_38588854 2021-01-13 04:31:39
  • 12.23MB yong_qi2015 2018-04-25 09:31:10
  • 3.05MB O_MMMM_O 2020-05-15 14:24:36
  • 5星
    376KB qq_36594694 2019-09-30 19:04:34
  • 2KB qq_27432295 2018-03-04 18:13:46
  • 4KB mrwang666 2019-03-06 17:18:59
  • 316KB weixin_43283267 2020-07-10 10:26:34
  • 634KB hewei930207 2017-11-27 23:57:33
  • 5星
    854KB adamshan 2017-10-26 22:40:06
  • 9.87MB baidu_24335673 2018-09-05 10:54:48
  • 5星
    1KB qq_27432295 2018-03-04 19:14:04
  • 5星
    128KB weixin_42696333 2018-08-14 20:41:00
  • 29.37MB huohulitju 2019-05-21 23:38:38
  • 4星
    1.67MB zyj1314157 2012-09-10 08:12:09
  • 前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波目标追踪的应用,我们在上一篇文章中还具体用Python实现了EKF,但是细心的同学会发现,EKF的效率确实很低,计算雅可比矩阵确实是一个很费时的操作,当问题...

    本文来源:http://blog.csdn.net/adamshan/article/details/78359048
    知乎的一篇相关文章:https://zhuanlan.zhihu.com/p/35729804

    前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波在目标追踪的应用,我们在上一篇文章中还具体用Python实现了EKF,但是细心的同学会发现,EKF的效率确实很低,计算雅可比矩阵确实是一个很费时的操作,当问题(非线性的)一旦变得复杂,其计算量就变得十分不可控制。在此再向大家接受一种滤波——无损卡尔曼滤波(Unscented Kalman Filter, UKF)

    通过上一篇文章,我们已经知道KF不适用于非线性系统,为了处理非线性系统,我们通过一阶泰勒展式来近似(用线性函数近似),这个方案直接的结果就是,我们对于具体的问题都要求解对应的一阶偏导(雅可比矩阵),求解雅可比矩阵本身就是费时的计算,而且我们上一篇还使用了Python而非C++,而且我们图省事还用了一个叫做numdifftools的库来实现雅可比矩阵的求解而不是自行推导,这一切都造成了我们上一次博客的代码执行效率奇低!显然现实应用里面我们的无人车是不能接受这种延迟的,我相信很多同学也和我一样讨厌求解雅可比矩阵,那么,就让我们来学习一种相对简单的状态估计算法——UKF。

    UKF使用的是统计线性化技术,我们把这种线性化的方法叫做无损变换(unscented transformation)这一技术主要通过n个在先验分布中采集的点(我们把它们叫sigma points)的线性回归来线性化随机变量的非线性函数,由于我们考虑的是随机变量的扩展,所以这种线性化要比泰勒级数线性化(EKF所使用的策略)更准确。和EKF一样,UKF也主要分为预测和更新。

    运动模型

    本篇我们继续使用CTRV运动模型,不了解该模型的同学可以去看上一篇博客,CTRV模型的具体形式如下:

    这里写图片描述

    当偏航角为0时:
    这里写图片描述

    在EKF中,我们将直线加速度和偏航角加速度的影响当作我们的处理噪声,并且假设他们是满足均值为 0,方差为 σa σ a σω˙ σ ω ˙ ,在这里,我们将噪声的影响直接考虑到我们的状态转移函数中来。至于函数中的不确定项 μa μ a μω˙ μ ω ˙ 我们后面再分析。

    非线性处理/测量模型

    我们知道我们在应用KF是面临的主要问题就是非线性处理模型(比如说CTRV)和非线性测量模型(RADAR测量)的处理。我们从概率分布的角度来描述这个问题:

    对于我们想要估计的状态,在k时刻满足均值为 xk|k x k | k ,方差为 Pk|k P k | k 这样的一个高斯分布,这个是我们在k时刻的后验(Posterior)(如果我们把整个卡尔曼滤波的过程迭代的来考虑的话),现在我们以这个后验为出发点,结合一定的先验知识(比如说CTRV运动模型)去估计我们在 k+1 k + 1 时刻的状态的均值和方差,这个过程就是卡尔曼滤波的预测,如果变换是线性的,那么预测的结果仍然是高斯分布,但是现实是我们的处理和测量模型都是非线性的,结果就是一个不规则分布,KF能够使用的前提就是所处理的状态是满足高斯分布的,为了解决这个问题,EKF是寻找一个线性函数来近似这个非线性函数,而UKF就是去找一个与真实分布近似的高斯分布

    UKF的基本思路就是: 近似非线性函数的概率分布要比近似非线性函数本身要容易

    那么如何去找一个与真实分布近似的高斯分布呢?——找一个与真实分布有着相同的均值和协方差的高斯分布。那么如何去找这样的均值和方差呢?——无损变换。

    无损变换

    计算一堆点(术语叫做sigma point),通过一定的手段产生的这些sigma点能够代表当前的分布,然后将这些点通过非线性函数(处理模型)变换成一些新的点,然后基于这些新的sigma点计算出一个高斯分布(带有权重地计算出来)如图:
    这里写图片描述这里写图片描述这里写图片描述

    预测

    由一个高斯分布产生sigma point

    通常,假定状态的个数为 n,我们会产生 2n+1 个sigma点,其中第一个就是我们当前状态的均值 μ ,sigma点集的均值的计算公式为:

    这里写图片描述

    其中的 λ λ 是一个超参数,根据公式,λ越大, sigma s i g m a 点就越远离状态的均值, λ λ 越小, sigma点就越靠近状态的均值。需要注意的是,在我们的CTRV模型中,状态数量 n 除了要包含5个状态以外,还要包含处理噪声 μa μ a μω˙ μ ω ˙ ,因为这些处理噪声对模型也有着非线性的影响。在增加了处理噪声的影响以后,我们的不确定性矩阵 P 就变成了:

    P=(P00Q) P = ( P ′ 0 0 Q )

    其中, P' P ′ 就是我们原来的不确定性矩阵(在CTRV模型中就是一个 5×5 的矩阵),Q是处理噪声的协方差矩阵,在CTRV模型中考虑到直线加速度核Q的形式为:

    Q=(σ2a00σ2ω˙) Q = ( σ a 2 0 0 σ ω ˙ 2 )

    其中的 σ2a σ a 2 , σ2ω˙ σ ω ˙ 2 和我们上一篇博客讲的一样。以上公式中还存在一个问题,那就是矩阵开平方根怎么计算的问题,同产情况下,我们求得是:

    A=P A = P

    其中,
    AAT=P A A T = P

    求解上式中的 A 是一个相对复杂的过程,但是如果 P 是对角矩阵的话,那么这个求解就会简化,在我们的实例中,P表示对估计状态的不确定性(协方差矩阵),我们会发现 P 基本上是对角矩阵(状态量之间的相关性几乎为0), 所以我们可以首先对 P 做 Cholesky分解(Cholesky decomposition) ,然后分解得到的矩阵的下三角矩阵就是我们要求的 A,在这里我们就不详细讨论了,在我们后面的实际例子中,我们直接调用库中相应的方法即可(注意:本次的实例我们换C++来实现,相比于Python,C++更加贴近我们实际的开发):

    c++
    MatrixXd A = P_aug.llt().matrixL();

    预测sigma point

    现在我们有sigma点集,我们就用非线性函数 g()来进行预测:

    χk+1=g(χk,μk) χ k + 1 = g ( χ k , μ k )

    需要注意的是,这里的输入 χk χ k 是一个 (7,15) 的矩阵(因为考虑了两个噪声量),但是输出 χk+1 χ k + 1 是一个(5,15) 的矩阵(因为这是预测的结果,本质上是基于运动模型的先验,先验中的均值不应当包含 a a ,ω˙这类不确定的量)

    预测均值和方差

    首先要计算出各个sigma点的权重,权重的计算公式为:

    这里写图片描述

    然后基于每个sigma点的权重去求新的分布的均值和方差:
    这里写图片描述

    其中 μ' μ ′ 即为我们基于CTRV模型预测的目标状态的先验分布的均值 xk+1|k x k + 1 | k , 它是sigma点集中每个点各个状态量的加权和, P' P ′ 即为先验分布的协方差(不确定性) Pk+1|k P k + 1 | k , 由每个sigma点的方差的加权和求得。至此,预测的部分也就走完了,下面进入了UKF的测量更新部分。

    测量更新

    预测测量(将先验映射到测量空间然后算出均值和方差)

    这篇博客继续使用上一篇(EKF)中的测量实验数据,那么我们知道,测量更新分为两个部分,LIDAR测量和RADAR测量,其中LIDAR测量模型本身就是线性的,所以我们重点还是放在RADAR测量模型的处理上面,RADAR的测量映射函数为:

    这里写图片描述

    这也是个非线性函数,我们用 h() 来表示它,再一次,我们使用无损转换来解决,但是这里,我们可以不用再产生sigma points了,我们可以直接使用预测出来的sigma点集,并且可以忽略掉处理噪声部分。那么对先验的非线性映射就可以表示为如下的sigma point预测(即预测非线性变换以后的均值和协方差):
    这里写图片描述

    和前面的文章一样,这里的 R 也是测量噪声,在这里我们直接将测量噪声的协方差加到测量协方差上是因为该噪声对系统没有非线性影响。在本例中,以RADAR的测量为例,那么测量噪声R为:

    这里写图片描述

    更新状态

    首先计算出sigma点集在状态空间和测量空间的互相关函数,计算公式如下:

    这里写图片描述

    后面我们就完整地按照卡尔曼滤波的更新步骤计算即可,先计算卡尔曼增益:
    这里写图片描述

    更新状态(也就是作出最后的状态估计):

    这里写图片描述

    其中 zk+1 z k + 1 是新得到的测量,而 zk+1|k z k + 1 | k 则是我们根据先验计算出来的在测量空间的测量。

    更新状态协方差矩阵:

    这里写图片描述

    至此,UKF的完整理论部分就介绍完了,具体的无损变换的意义和推导由于太过于理论化了,大家如果感兴趣可以去看这几片文献:

    http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.136.6539&rep=rep1&type=pdf

    https://www.cse.sc.edu/~terejanu/files/tutorialUKF.pdf

    http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/pdf/slam06-ukf-4.pdf
    UKF实例

    我们继续基于上一篇文章的数据来做状态估计的实例,不过,为了更加接近实际实际的应用,我们本节采用C++实现。由于本节使用的C++代码量相对较大,而且为多文件的项目,代码就不再一一贴出,所有代码我都已经上传至如下地址:

    http://download.csdn.net/download/adamshan/10041096
    运行-效果

    首先解压,编译:

    mkdir build
    cd build
    cmake ..
    make

    运行:

    ./ukf ../data/data_synthetic.txt ../data/output.txt

    得到计算的结果:

    Accuracy - RMSE:
    0.0723408
    0.0821208
     0.342265
      0.23017

    我们发现,这个UKF要比我们上一篇博客写的EKF的效率要高得多得多。

    我们执行可视化的python脚本看一下效果:

    cd ../data
    python plot.py

    得到如下结果:
    这里写图片描述

    放大一点:

    这里写图片描述

    UKF在我们这个问题中的执行效率和估计精度都高于我们上一篇文章中实现的EKF,下面就让我们看一下核心代码。
    核心代码

    首先是预测,预测主要包含三部分,分别是:

    • 产生sigma点集
    • 基于CTRV模型预测sigma点集
    • 计算新的均值核方差

    产生sigma点集

    void UKF::AugmentedSigmaPoints(MatrixXd *Xsig_out) {
    
        //create augmented mean vector
        VectorXd x_aug = VectorXd(7);
    
        //create augmented state covariance
        MatrixXd P_aug = MatrixXd(7, 7);
    
        //create sigma point matrix
        MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
    
        //create augmented mean state
        //create augmented covariance matrix
        //create square root matrix
        //create augmented sigma points
        x_aug.head(5) = x_;
        x_aug(5) = 0;
        x_aug(6) = 0;
    
        P_aug.fill(0.0);
        P_aug.topLeftCorner(5, 5) = P_;
        P_aug(5,5) = std_a_*std_a_;
        P_aug(6,6) = std_yawdd_*std_yawdd_;
    
        MatrixXd A = P_aug.llt().matrixL();
    
        //create augmented sigma points
        Xsig_aug.col(0)  = x_aug;
        for (int i = 0; i< n_aug_; i++)
        {
            Xsig_aug.col(i+1)       = x_aug + sqrt(lambda_+n_aug_) * A.col(i);
            Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_+n_aug_) * A.col(i);
        }
    
        //write result
        *Xsig_out = Xsig_aug;
    }

    基于CTRV模型预测sigma点集

    void UKF::SigmaPointPrediction(MatrixXd &Xsig_aug, double delta_t) {
    
        for(int i =0; i < (2 * n_aug_ + 1); i++){
            VectorXd input_x = Xsig_aug.col(i);
            float px = input_x[0];
            float py = input_x[1];
            float v = input_x[2];
            float psi = input_x[3];
            float psi_dot = input_x[4];
            float mu_a = input_x[5];
            float mu_psi_dot_dot = input_x[6];
    
            VectorXd term2 = VectorXd(5);
            VectorXd term3 = VectorXd(5);
    
            VectorXd result = VectorXd(5);
            if(psi_dot < 0.001){
                term2 << v * cos(psi) * delta_t, v * sin(psi) * delta_t, 0, psi_dot * delta_t, 0;
                term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a,
                        0.5 * delta_t*delta_t * sin(psi) * mu_a,
                        delta_t * mu_a,
                        0.5 * delta_t*delta_t * mu_psi_dot_dot,
                        delta_t * mu_psi_dot_dot;
                result = Xsig_aug.col(i).head(5) + term2 + term3;
            } else{
                term2 << (v/psi_dot) * (sin(psi + psi_dot * delta_t) - sin(psi)),
                        (v/psi_dot) * (-cos(psi + psi_dot * delta_t) + cos(psi)),
                        0,
                        psi_dot * delta_t,
                        0;
    
                term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a,
                        0.5 * delta_t*delta_t * sin(psi) * mu_a,
                        delta_t * mu_a,
                        0.5 * delta_t*delta_t * mu_psi_dot_dot,
                        delta_t * mu_psi_dot_dot;
                result = Xsig_aug.col(i).head(5) + term2 + term3;
            }
    
            Xsig_pred_.col(i) = result;
        }
    }

    计算新的均值核方差

    void UKF::PredictMeanAndCovariance() {
        x_.fill(0.0);
        for(int i=0; i<2*n_aug_+1; i++){
            x_ = x_+ weights_[i] * Xsig_pred_.col(i);
        }
    
        P_.fill(0.0);
        for(int i=0;  i<2*n_aug_+1; i++){
            VectorXd x_diff = Xsig_pred_.col(i) - x_;
            while (x_diff[3]> M_PI)
                x_diff[3] -= 2.*M_PI;
            while (x_diff[3] <-M_PI)
                x_diff[3]+=2.*M_PI;
            P_ = P_ + weights_[i] * x_diff * x_diff.transpose();
        }
    }

    测量更新主要分为:

    • 预测LIDAR测量
    • 预测RADAR测量
    • 更新状态

    预测LIDAR测量

    void UKF::PredictLaserMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {
        for(int i=0; i < 2*n_aug_+1; i++){
            float px = Xsig_pred_.col(i)[0];
            float py = Xsig_pred_.col(i)[1];
    
            VectorXd temp = VectorXd(n_z);
            temp << px, py;
            Zsig.col(i) = temp;
        }
    
        z_pred.fill(0.0);
        for(int i=0; i < 2*n_aug_+1; i++){
            z_pred = z_pred + weights_[i] * Zsig.col(i);
        }
    
        S.fill(0.0);
        for(int i=0; i < 2*n_aug_+1; i++){
            //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;
    
            S = S + weights_[i] * z_diff * z_diff.transpose();
        }
        S = S + R_laser_;
    }

    预测RADAR测量

    void UKF::PredictRadarMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {
        for(int i=0; i < 2*n_aug_+1; i++){
            float px = Xsig_pred_.col(i)[0];
            float py = Xsig_pred_.col(i)[1];
            float v = Xsig_pred_.col(i)[2];
            float psi = Xsig_pred_.col(i)[3];
            float psi_dot = Xsig_pred_.col(i)[4];
    
            float temp = px * px + py * py;
            float rho = sqrt(temp);
            float phi = atan2(py, px);
            float rho_dot;
            if(fabs(rho) < 0.0001){
                rho_dot = 0;
            } else{
                rho_dot =(px * cos(psi) * v + py * sin(psi) * v)/rho;
            }
    
            VectorXd temp1 = VectorXd(3);
            temp1 << rho, phi, rho_dot;
            Zsig.col(i) = temp1;
        }
    
        z_pred.fill(0.0);
        for(int i=0; i < 2*n_aug_+1; i++){
            z_pred = z_pred + weights_[i] * Zsig.col(i);
        }
    
        S.fill(0.0);
        for(int i=0; i < 2*n_aug_+1; i++){
            //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;
    
            //angle normalization
            while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
            while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
            S = S + weights_[i] * (Zsig.col(i) - z_pred) * (Zsig.col(i) - z_pred).transpose();
        }
        S = S + R_radar_;
    }

    更新状态

    void UKF::UpdateState(VectorXd &z, VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) {
    
        //create matrix for cross correlation Tc
        MatrixXd Tc = MatrixXd(n_x_, n_z);
    
        //calculate cross correlation matrix
        //calculate Kalman gain K;
        //update state mean and covariance matrix
    
        Tc.fill(0.0);
        for(int i=0; i < 2*n_aug_+1; i++){
            VectorXd x_diff = Xsig_pred_.col(i) - x_;
    
            while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
            while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
    
            //residual
            VectorXd z_diff = Zsig.col(i) - z_pred;
            if(n_z == 3){
                //angle normalization
                while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
                while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
            }
            Tc = Tc + weights_[i] * x_diff * z_diff.transpose();
        }
    
        MatrixXd K = MatrixXd(5, 3);
        K = Tc * S.inverse();
    
        VectorXd y = z - z_pred;
        //angle normalization
        if(n_z == 3){
            while (y(1)> M_PI) y(1)-=2.*M_PI;
            while (y(1)<-M_PI) y(1)+=2.*M_PI;
        }
        x_ = x_ + K * y;
        P_ = P_ - K * S * K.transpose();
    }

    以上就是我们的UKF的核心算法实现了,可能光看这些核心代码还是没办法理解,所以感兴趣的童鞋就去下载来再详细研究把~

    上一篇文章我买了个关子,没有贴出把结果可视化的代码,本次代码已经一本包含在项目里(具体见/data/plot.py)

    前面三篇文章都介绍了卡尔曼滤波相关的算法,大家应该已经看出卡尔曼滤波这类贝叶斯滤波器在无人驾驶汽车系统哦的重要性了,卡尔曼滤波作为重要的状态估计算法,是无人驾驶的必备技能点,所以相关的扩展阅读大家可以多多开展,本系列暂时就不再讨论了,下面我想向大家介绍应用于无人驾驶中的控制算法,控制作为自动化中重要的技术,有着很多的分支,后面的文章我会继续由理论到实践逐一向大家介绍相关技术~

    展开全文
    yuxuan20062007 2018-06-30 20:15:07
  • 5星
    9.27MB weixin_42696333 2021-09-10 18:39:20
  • 393KB weixin_38677190 2021-05-23 17:32:50
  • 6.38MB ldf183 2016-08-24 17:51:09
  • 3星
    129KB narutowz 2018-09-19 08:35:47
  • 5星
    128.31MB weixin_42116794 2021-04-12 11:58:23
  • 最近学习了一下多目标跟踪,看了看MathWorks的关于Motion-Based Multiple Object Tracking的Documention。 实现硬件条件: VS2015+opencv3.10 opencv2.4.9需要修改几处,需要的可留下邮箱,小博立马回复 ...

    最近学习了一下多目标跟踪,看了看MathWorks的关于Motion-Based Multiple Object Tracking的Documention。
    实现硬件条件:
    VS2015+opencv3.10
    opencv2.4.9需要修改几处,需要的可留下邮箱,小博立马回复
    下面贴出我的车辆追踪效果图吧,当然大家可以用来适用于别的场景了。其中红点代表车的质心,绿线代表轨迹和轮廓。
    四辆小汽车哦
    两辆SUV

    官网链接:http://cn.mathworks.com/help/vision/examples/motion-based-multiple-object-tracking.html?s_tid=gn_loc_drop
    经过小博两天晚上的加工,终于将这篇论文用C++和opencv给实现出来了。这种方法用于静止背景下的多目标检测与跟踪。效果非常不错,特别适用于无人机上。

    程序可以分为两部分,1.每一帧检测运动objects;
    2.实时的将检测得到的区域匹配到相同一个物体;检测部分,用的是基于高斯混合模型的背景剪除法;

       参考链接:http://blog.pluskid.org/?p=39
    

    所谓单高斯模型,就是用多维高斯分布概率来进行模式分类,其中μ用训练样本均值代替,Σ用样本方差代替,X为d维的样本向量。通过高斯概率公式就可以得出类别C属于正(负)样本的概率。而混合高斯模型(GMM)就是数据从多个高斯分布中产生的。每个GMM由K个高斯分布线性叠加而 P(x)=Σp(k)*p(x|k) 相当于对各个高斯分布进行加权(权系数越大,那么这个数据属于这个高斯分布的可能性越大)
    而在实际过程中,我们是在已知数据的前提下,对GMM进行参数估计,具体在这里即为图片训练一个合适的GMM模型。那么在前景检测中,我们会取静止背景(约50帧图像)来进行GMM参数估计,进行背景建模。分类域值官网取得0.7,经验取值0.7-0.75可调。这一步将会分离前景和背景,输出为前景二值掩码。
    然后进行形态学运算,并通过函数返回运动区域的centroids和bboxes,完成前景检测部分。

    跟踪部分,用的是卡尔曼滤波。卡尔曼是一个线性估计算法,可以建立帧间bboxs的关系。

    跟踪分为5种状态: 1,新目标出现 2,目标匹配 3,目标遮挡 4,目标分离 5,目标消失。

    卡尔曼原理在这儿我就不贴了,网上很多。

    状态方程: X(k+1)=A(K+1,K)X(K)+w(K) 其中 X(k)=[x(k),y(k),w(k),h(k),v(k)], x,y,w,h,分别表示bboxs的横纵坐标,长,宽。

    观测方程: Z(k)=H(k)X(k)+v(k) w(k), v(k),不相关的高斯白噪声。

    定义好了观测方程与状态方程之后就可以用卡尔曼滤波器实现运动目标的跟踪,步骤如下:

    1)计算运动目标的特征信息(运动质心,以及外接矩形)。

    2)用得到的特征信息初始化卡尔曼滤波器(开始时可以初始为0)。

    3)用卡尔曼滤波器对下一帧中对应的目标区域进行预测,当下一帧到来时,在预测区域内进行目标匹配。

    4)如果匹配成功,则更新卡尔曼滤波器

    在匹配的过程中,使用的是匈牙利匹配算法,匈牙利算法在这里有很好的介绍:http://blog.csdn.net/pi9nc/article/details/11848327
    

    匈牙利匹配算法在此处是将新一帧图片中检测到的运动物体匹配到对应的轨迹。匹配过程是通过最小化卡尔曼预测得到的质心与检测到的质心之间的欧氏距离之和实现的

    具体可以分为两步:

    1, 计算损失矩阵,大小为[M N],其中,M是轨迹数目,N是检测到的运动物体数目。

    2, 求解损失矩阵

    主要思路就是这么多,下面贴上C++的demo,大家可以跑一跑。

    #include <iostream>  
    #include <opencv2/opencv.hpp>  
    #include <opencv2/video/background_segm.hpp>  
    using namespace std;
    using namespace cv;
    
    #define drawCross( img, center, color, d )\
    line(img, Point(center.x - d, center.y - d), Point(center.x + d, center.y + d), color, 2, CV_AA, 0);\
    line(img, Point(center.x + d, center.y - d), Point(center.x - d, center.y + d), color, 2, CV_AA, 0);
    
    vector<Point> mousev, kalmanv;
    
    
    cv::KalmanFilter KF;
    cv::Mat_<float> measurement(2, 1);
    Mat_<float> state(4, 1); // (x, y, Vx, Vy)  
    int incr = 0;
    string num2str(int i)
    {
    	stringstream ss;
    	ss << i;
    	return ss.str();
    }
    
    void initKalman(float x, float y)
    {
    	// Instantate Kalman Filter with  
    	// 4 dynamic parameters and 2 measurement parameters,  
    	// where my measurement is: 2D location of object,  
    	// and dynamic is: 2D location and 2D velocity.  
    	KF.init(4, 2, 0);
    
    	measurement = Mat_<float>::zeros(2, 1);
    	measurement.at<float>(0, 0) = x;
    	measurement.at<float>(0, 0) = y;
    
    
    	KF.statePre.setTo(0);
    	KF.statePre.at<float>(0, 0) = x;
    	KF.statePre.at<float>(1, 0) = y;
    
    	KF.statePost.setTo(0);
    	KF.statePost.at<float>(0, 0) = x;
    	KF.statePost.at<float>(1, 0) = y;
    
    	setIdentity(KF.transitionMatrix);
    	setIdentity(KF.measurementMatrix);
    	setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise  
    	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    	setIdentity(KF.errorCovPost, Scalar::all(.1));
    }
    
    Point kalmanPredict()
    {
    	Mat prediction = KF.predict();
    	Point predictPt(prediction.at<float>(0), prediction.at<float>(1));
    
    	KF.statePre.copyTo(KF.statePost);
    	KF.errorCovPre.copyTo(KF.errorCovPost);
    
    	return predictPt;
    }
    
    Point kalmanCorrect(float x, float y)
    {
    	measurement(0) = x;
    	measurement(1) = y;
    	Mat estimated = KF.correct(measurement);
    	Point statePt(estimated.at<float>(0), estimated.at<float>(1));
    	return statePt;
    }
    
    int main()
    {
    	Mat frame, thresh_frame;
    	vector<Mat> channels;
    	VideoCapture capture("1.avi");
    	if (!capture.isOpened())
    		cout << "hello";
    	//vector<Vec4i> hierarchy;
    	vector<vector<Point> > contours;
    
    	// cv::Mat frame;  
    	cv::Mat back,img;
    	cv::Mat fore;
    	cv::Ptr<BackgroundSubtractorMOG2> bg = createBackgroundSubtractorMOG2();
    	//cv::BackgroundSubtractorMOG2 bg;
    	// bg->setNMixtures(3);
    	// bg->apply(img, mask);
    	//bg.nmixtures = 3;//nmixtures  
    	//bg.bShadowDetection = false;  
    	int incr = 0;
    	int track = 0;
    	bool update_bg_model = true;
    	//capture.open("1.avi");
    
    	if (!capture.isOpened())
    		cerr << "Problem opening video source" << endl;
    
    
    	mousev.clear();
    	kalmanv.clear();
    
    	initKalman(0, 0);
    
    	while ((char)waitKey(1) != 'q' && capture.grab())
    	{
    
    		Point s, p;
    
    		capture.retrieve(frame);
    		//bg(img, fgmask, update_bg_model ? -1 : 0);
    		//bg->operator ()(frame, fore);
    		bg->apply(frame, fore, update_bg_model ? -1 : 0);
    		bg->getBackgroundImage(back);
    		erode(fore, fore, Mat());
    		erode(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    		dilate(fore, fore, Mat());
    
    		cv::normalize(fore, fore, 0, 1., cv::NORM_MINMAX);
    		cv::threshold(fore, fore, .5, 1., CV_THRESH_BINARY);
    
    
    		split(frame, channels);
    		add(channels[0], channels[1], channels[1]);
    		subtract(channels[2], channels[1], channels[2]);
    		threshold(channels[2], thresh_frame, 50, 255, CV_THRESH_BINARY);
    		medianBlur(thresh_frame, thresh_frame, 5);
    
    		//       imshow("Red", channels[1]);  
    		findContours(fore, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
    		vector<vector<Point> > contours_poly(contours.size());
    		vector<Rect> boundRect(contours.size());
    
    		Mat drawing = Mat::zeros(thresh_frame.size(), CV_8UC1);
    		for (size_t i = 0; i < contours.size(); i++)
    		{
    			//          cout << contourArea(contours[i]) << endl;  
    			if (contourArea(contours[i]) > 500)
    				drawContours(drawing, contours, i, Scalar::all(255), CV_FILLED, 8, vector<Vec4i>(), 0, Point());
    		}
    		thresh_frame = drawing;
    
    		findContours(fore, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
    
    		drawing = Mat::zeros(thresh_frame.size(), CV_8UC1);
    		for (size_t i = 0; i < contours.size(); i++)
    		{
    			//          cout << contourArea(contours[i]) << endl;  
    			if (contourArea(contours[i]) > 3000)
    				drawContours(drawing, contours, i, Scalar::all(255), CV_FILLED, 8, vector<Vec4i>(), 0, Point());
    		}
    		thresh_frame = drawing;
    
    		// Get the moments  
    		vector<Moments> mu(contours.size());
    		for (size_t i = 0; i < contours.size(); i++)
    		{
    			mu[i] = moments(contours[i], false);
    		}
    
    		//  Get the mass centers:  
    		vector<Point2f> mc(contours.size());
    		for (size_t i = 0; i < contours.size(); i++)
    
    		{
    			mc[i] = Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    
    
    			/*
    			for(size_t i = 0; i < mc.size(); i++)
    			{
    
    			//       drawCross(frame, mc[i], Scalar(255, 0, 0), 5);
    			//measurement(0) = mc[i].x;
    			//measurement(1) = mc[i].y;
    
    
    			//        line(frame, p, s, Scalar(255,255,0), 1);
    
    			//          if (measurement(1) <= 130 && measurement(1) >= 120) {
    			//            incr++;
    			//         cout << "Conter " << incr << " Loation " << measurement(1) << endl;
    			//   }
    			}*/
    		}
    
    
    		for (size_t i = 0; i < contours.size(); i++)
    		{
    			approxPolyDP(Mat(contours[i]), contours_poly[i], 3, true);
    			boundRect[i] = boundingRect(Mat(contours_poly[i]));
    
    		}
    
    		p = kalmanPredict();
    		//        cout << "kalman prediction: " << p.x << " " << p.y << endl;  
    		mousev.push_back(p);
    		string text;
    		for (size_t i = 0; i < contours.size(); i++)
    		{
    			if (contourArea(contours[i]) > 1000)
    			{
    				rectangle(frame, boundRect[i].tl(), boundRect[i].br(), Scalar(0, 255, 0), 2, 8, 0);
    				Point pt = Point(boundRect[i].x, boundRect[i].y+boundRect[i].height+15);
    				Point pt1 = Point(boundRect[i].x, boundRect[i].y - 10);
    				Point center = Point(boundRect[i].x + (boundRect[i].width / 2), boundRect[i].y + (boundRect[i].height / 2));
    				cv::circle(frame, center, 8, Scalar(0, 0, 255), -1, 1, 0);
    				Scalar color = CV_RGB(255, 0, 0);
    				text = num2str(boundRect[i].width) + "*" + num2str(boundRect[i].height);
    			    putText(frame,"car", pt, CV_FONT_HERSHEY_DUPLEX, 1.0f, color);
    				putText(frame,text, pt1, CV_FONT_HERSHEY_DUPLEX, 1.0f, color);
    
    				s = kalmanCorrect(center.x, center.y);
    				drawCross(frame, s, Scalar(255, 255, 255), 5);
    
    				if (s.y <= 130 && p.y > 130 && s.x > 15)
    				{
    					incr++;
    					cout << "Counter " << incr << endl;
    				}
    
    
    				for (int i = mousev.size() - 20; i < mousev.size() - 1; i++)
    				{
    					line(frame, mousev[i], mousev[i + 1], Scalar(0, 255, 0), 1);
    				}
    
    			}
    		}
    
    
    		imshow("Video", frame);
    	//	imshow("Red", channels[2]);
    	//	imshow("Binary", thresh_frame);
    	}
    	return 0;
    }
    
    展开全文
    xiao__run 2017-08-22 12:20:23
  • 作者:博客如 来源:CSDN ...版权声明:本文为博主原创文章,转载请附上博文链接! 转载只为日后好用,原创在上方. ...写博客记录下做过的工作,毕竟好...卡尔曼滤波算法的博客很多,白巧克力亦唯心http://blog.csdn....

    作者:博客如 
    来源:CSDN 
    原文:https://blog.csdn.net/m0_38089090/article/details/79523784 
    版权声明:本文为博主原创文章,转载请附上博文链接!

     

    转载只为日后好用,原创在上方.

     

    写博客记录下做过的工作,毕竟好记性不如烂笔头。

            卡尔曼滤波算法的博客很多,白巧克力亦唯心http://blog.csdn.net/heyijia0327的阐述应该是较为通俗易懂的,他的理论部分是参考Greg Welch & Gary Bishop. << An Introduction to the Kalman Filter >>,实例小车部分类似文章Ramsey Faragher. << Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation >>,网上都可以找到。

            以下是学习过程中的笔记,略潦草,省略了具体的公式推导,呈现了卡尔曼滤波的基本算法流程(需要事先了解相关概念)。


     

    matlab仿真可以看http://blog.csdn.net/heyijia0327/article/details/17667341,因为最近在复习C++,所以用C++写了一个,简陋之处望指正。

            应用背景是匀加速小车,该线性系统的状态差分方程为

            对小车进行建模,ft为合力,小车的状态方程表示为

            矩阵形式表示为

            具体程序如下,某些参数定义见注释。
     

    // Kalme.cpp : 定义控制台应用程序的入口点。
    //
     
    #include "stdafx.h"
    #include <iostream>
    #include <fstream>
    #include <string>
    #include <vector>
    #include <Eigen/Dense>//包含Eigen矩阵运算库,用于矩阵计算
    #include <cmath>
    #include <limits>//用于生成随机分布数列
     
    using namespace std;
    using Eigen::MatrixXd;
    int _tmain(int argc, _TCHAR* argv[])
    {
        //""中是txt文件路径,注意:路径要用//隔开
        ofstream fout("..//result.txt");
        
        double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数
     
        const double delta_t = 0.1;//控制周期,100ms
        const int num = 100;//迭代次数
        const double acc = 10;//加速度,ft/m
     
        MatrixXd A(2,2);
        A(0,0) = 1;
        A(1,0) = 0;
        A(0,1) = delta_t;
        A(1,1) = 1;
     
        MatrixXd B(2,1);
        B(0,0) = pow(delta_t,2)/2;
        B(1,0) = delta_t;
     
        MatrixXd H(1,2);//测量的是小车的位移,速度为0
        H(0,0) = 1;
        H(0,1) = 0;
        
        MatrixXd Q(2,2);//过程激励噪声协方差,假设系统的噪声向量只存在速度分量上,且速度噪声的方差是一个常量0.01,位移分量上的系统噪声为0
        Q(0,0) = 0;
        Q(1,0) = 0;
        Q(0,1) = 0;
        Q(1,1) = 0.01;
     
        MatrixXd R(1,1);//观测噪声协方差,测量值只有位移,它的协方差矩阵大小是1*1,就是测量噪声的方差本身。
        R(0,0) = 10;
     
        //time初始化,产生时间序列
        vector<double> time(100, 0);
        for(decltype(time.size()) i = 0; i != num; ++i){
            time[i] = i * delta_t;
            //cout<<time[i]<<endl;
        }
     
        MatrixXd X_real(2,1);
        vector<MatrixXd> x_real, rand;
        //生成高斯分布的随机数
        for(int i = 0; i<100;++i){
            MatrixXd a(1,1);
            a(0,0) = generateGaussianNoise(0,sqrt(10));
            rand.push_back(a);
        }
        //生成真实的位移值
        for(int i = 0; i < num; ++i){
            X_real(0,0) = 0.5 * acc * pow(time[i],2);
            X_real(1,0) = 0;
            x_real.push_back(X_real);
        }
     
        //变量定义,包括状态预测值,状态估计值,测量值,预测状态与真实状态的协方差矩阵,估计状态和真实状态的协方差矩阵,初始值均为零
        MatrixXd X_evlt = MatrixXd::Constant(2,1,0), X_pdct = MatrixXd::Constant(2,1,0), Z_meas = MatrixXd::Constant(1,1,0), 
            Pk = MatrixXd::Constant(2,2,0), Pk_p = MatrixXd::Constant(2,2,0), K = MatrixXd::Constant(2,1,0);
        vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;
        x_evlt.push_back(X_evlt);
        x_pdct.push_back(X_pdct);
        z_meas.push_back(Z_meas);
        pk.push_back(Pk);
        pk_p.push_back(Pk_p);
        k.push_back(K);
     
        //开始迭代
        for(int i = 1; i < num; ++i){
            //预测值
            X_pdct = A * x_evlt[i-1] + B * acc;
            x_pdct.push_back(X_pdct);
            //预测状态与真实状态的协方差矩阵,Pk'
            Pk_p = A * pk[i-1] * A.transpose() + Q;
            pk_p.push_back(Pk_p);
            //K:2x1
            MatrixXd tmp(1,1);
            tmp = H * pk_p[i] * H.transpose() + R;
            K = pk_p[i] * H.transpose() * tmp.inverse();
            k.push_back(K);
            //测量值z
            Z_meas = H * x_real[i] + rand[i];
            z_meas.push_back(Z_meas);
            //估计值
            X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
            x_evlt.push_back(X_evlt);
            //估计状态和真实状态的协方差矩阵,Pk
            Pk = (MatrixXd::Identity(2,2) - k[i] * H) * pk_p[i];
            pk.push_back(Pk);
        }
        
        cout<<"含噪声测量"<<"  "<<"后验估计"<<"  "<<"真值"<<"  "<<endl;
        for(int i = 0; i < num; ++i){
            //cout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;
            fout<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](0,0)<<endl;//输出到txt文档,用于matlab绘图
            //cout<<k[i](1,0)<<endl;
            //fout<<rand[i](0,0)<<endl;
            //fout<<x_pdct[i](0,0)<<endl;
        }
     
        fout.close();
     
        return 0;
    }
     
    //生成高斯分布随机数的函数,网上找的
    double generateGaussianNoise(double mu, double sigma)
    {
        const double epsilon = std::numeric_limits<double>::min();
        const double two_pi = 2.0*3.14159265358979323846;
     
        static double z0, z1;
        static bool generate;
        generate = !generate;
     
        if (!generate)
           return z1 * sigma + mu;
     
        double u1, u2;
        do
         {
           u1 = rand() * (1.0 / RAND_MAX);
           u2 = rand() * (1.0 / RAND_MAX);
         }
        while ( u1 <= epsilon );
     
        z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
        z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
        return z0 * sigma + mu;
    }

     

    将数据结果加载到matlab,绘图如下所示,可以看到红色的估计值和黑色的真实值基本吻合,蓝色的为含噪声测量值。

     

    注:Eigen矩阵运算库的使用,从官网下载zip压缩包,解压,在工程项目,VC++目录,包含目录中添加解压的文件夹所在目录即可

    展开全文
    weixin_41856133 2019-07-18 20:46:40

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,712
精华内容 684
关键字:

卡尔曼滤波目标跟踪c++

c++ 订阅