精华内容
下载资源
问答
  • 假设匀速运动模型,对上一帧的MapPoints进行跟踪,用上一帧位姿和速度估计当前帧位姿。 (1)上一帧地图点投影到当前帧,完成匹配。 int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,...

    这里是Tracking部分的第二部分,详细讲述各分支的代码及其实现原理。


    单目初始化

    MonocularInitialization()

    目标:从初始的两帧单目图像中,对SLAM系统进行初始化(得到初始两帧的匹配,相机初始位姿,初始MapPoints),以便之后进行跟踪。

    方式:并行得计算基础矩阵E和单应矩阵H,恢复出最开始两帧相机位姿;三角化得到MapPoints的深度,获得点云地图。

    寻找匹配特征点

    单目的初始化有专门的初始化器,只有连续的两帧特征点均>100个才能够成功构建初始化器。

    完成前两帧特征点的匹配

    ORBmatcher matcher(0.9,true);
    int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

    mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点。

    这里的ORBmatcher类后面有机会再详细介绍。功能就是ORB特征点的匹配。

    从匹配点中恢复初始相机位姿

    在得到超过100个匹配点对后,运用RANSAC 8点法同时计算单应矩阵H和基础矩阵E,并选择最合适的结果作为相机的初始位姿。

    mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
    mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)

    Initializer类是初始化类。下面要详细讲一下类成员函数 intilize(),能够根据当前帧和两帧匹配点计算出相机位姿R|t ,以及三角化得到 3D特征点。

    Initializer-> intilize()

    (1)调用多线程分别用于计算fundamental matrix和homography

        // 计算homograpy并打分
        thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
        // 计算fundamental matrix并打分
        thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
    
        // Wait until both threads have finished
        threadH.join();
        threadF.join();

    (2)计算得分比例,选取某个模型进行恢复R|t

        float RH = SH/(SH+SF);
    
        // 步骤5:从H矩阵或F矩阵中恢复R,t
        if(RH>0.40)
            return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
        else //if(pF_HF>0.6)
            return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    具体的:

    FindHomography或FindFundamental

    (1) 计算单应矩阵cv::Mat Hn = ComputeH21(vPn1i,vPn2i);,并进行评分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);,分别通过单应矩阵将第一帧的特征点投影到第二帧,以及将第二帧的特征点投影到第一帧,计算两次重投影误差,叠加作为评分SH。

    (2) 计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);,并进行评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);,两次重投影误差叠加作为评分SF。

    创建初始地图

    如果恢复相机初始位姿成功,那么我们能得到前两帧图像的位姿,以及三角测量后的3维地图点。之后进行如下操作:

                // Set Frame Poses
                // 将初始化的第一帧作为世界坐标系,因此第一帧变换矩阵为单位矩阵
                mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
                // 由Rcw和tcw构造Tcw,并赋值给mTcw,mTcw为世界坐标系到该帧的变换矩阵
                cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
                Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
                tcw.copyTo(Tcw.rowRange(0,3).col(3));
                mCurrentFrame.SetPose(Tcw);
    
                // 步骤6:将三角化得到的3D点包装成MapPoints
                // Initialize函数会得到mvIniP3D,
                // mvIniP3D是cv::Point3f类型的一个容器,是个存放3D点的临时变量,
                // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
                CreateInitialMapMonocular();

     先将当前帧位姿Tcw设置好,之后CreateInitialMapMonocular () 创建初始地图,并进行相应的3D点数据关联操作:

    CreateInitialMapMonocular ()

    (1)创建关键帧

    因为只有前两帧,所以将这两帧都设置为关键帧,并计算对应的BoW,并在地图中插入这两个关键帧。

    (2)创建地图点+数据关联

    将3D点包装成地图点,将地图点与关键帧,地图进行数据关联。

    • 关键帧与地图点关联

    一个地图点可被多个关键帧观测到,将观测到这个地图点的关键帧与这个地图点进行关联;关键帧能够观测到很多地图点,将这些地图点与该关键帧关联。

            //关键帧上加地图点
            pKFini->AddMapPoint(pMP,i);
            pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
            //地图点的属性:能够观测到的关键帧
            pMP->AddObservation(pKFini,i);
            pMP->AddObservation(pKFcur,mvIniMatches[i]);

    地图点与关键帧上的特征点关联后,计算最合适的描述子来描述该地图点,用于之后跟踪的匹配。

            // b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
            pMP->ComputeDistinctiveDescriptors();
            // c.更新该MapPoint平均观测方向以及观测距离的范围
            pMP->UpdateNormalAndDepth();
    • 关键帧的特征点与地图点关联

    一个关键帧上特征点由多个地图点投影而成,将关键帧与地图点关联。

            //Fill Current Frame structure
            mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
            mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
    • 关键帧与关键帧关联

    关键帧之间会共视一个地图点,如果共视的地图点个数越多,说明这两个关键帧之间的联系越紧密。对于某个关键帧,统计其与其他关键帧共视的特征点个数,如果大于某个阈值,那么将这两个关键帧进行关联。

        pKFini->UpdateConnections();
        pKFcur->UpdateConnections();

    (3)Global BA

    ​ 对上述只有两个关键帧和地图点的地图进行全局BA。

        // 步骤5:BA优化
        Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    (4) 地图尺寸初始化(中值深度为1)

    ​ 由于单目SLAM的的地图点坐标尺寸不是真实的,比如x=1可能是1m也可能是1cm。选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化


    跟踪匀速运动模型

    TrackWithMotionModel()

    假设匀速运动模型,对上一帧的MapPoints进行跟踪,用上一帧位姿和速度估计当前帧位姿。

    (1)上一帧地图点投影到当前帧,完成匹配。

    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

    这是ORBmatcher类中定义的类成员函数SearchByProjection()。

    如果匹配点不够20个,扩大面积继续投影。

    (2)优化相机位姿,BA算法,最小二乘法。

    Optimizer::PoseOptimization(&mCurrentFrame);

    (3)剔除优化后的outlier匹配点

    跟踪参考关键帧模型

    TrackReferenceKeyFrame()

    在运动模型还未建立或者刚完成重定位时,通过BoW进行当前帧与参考关键帧特征点匹配,进而优化位姿

    (1)将当前帧的描述子转化为BoW向量,与关键帧的BoW进行匹配。

    mCurrentFrame.ComputeBoW();
    int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

    SearchByBoW()是ORBmatcher的类成员函数。

    (2)将上一帧的位姿态作为当前帧位姿的初始值

    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值,在PoseOptimization可以收敛快一些

    (3)通过优化3D-2D的重投影误差来获得位姿

    (4)剔除优化后的outlier匹配点(MapPoints)


    重定位

    Relocalization()

    当跟踪失败,将当前帧转成BoW向量向所有关键帧中查找与当前帧有充足匹配点的候选帧,Ransac迭代,PnP求解位姿,再通过最小化重投影误差对位姿进行优化。

    (1)计算当前帧的BoW映射

    (2)找到与当前帧相似的候选关键帧

    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

    (3)匹配当前帧与找到的候选关键帧,计算相机位姿Tcw(RANSAC+PNP算法)

    int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

     通过BoW向量匹配上,之后初始化PnPSolver,使用RANSAC+PNP算法完成位姿估计。

                    PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                    pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                    vpPnPsolvers[i] = pSolver;
                    nCandidates++;
               PnPsolver* pSolver = vpPnPsolvers[i];
                cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

    (4)优化相机位姿Tcw

     

    总结:以上三种方法都是基于帧间匹配,优化3D-2D重投影误差得到当前帧的初始位姿。这样得到的初始位姿不是可靠的,其只利用了两帧数据的信息,如果前一帧质量太差,得到的位姿可信度低。因此为了利用更多的信息,需要进一步将当前帧与局部地图进行匹配和优化,也就是TrackLocalMap() 。


    跟踪局部地图

    TrackLocalMap()

    对相机位姿有个基本估计和初始特征匹配,将局部地图投影到当前帧寻找更多对应的匹配地图点MapPoints,优化相机位姿。

    随着相机运动,我们向局部地图添加新的关键帧和3D地图点来维护局部地图,这样,即使跟踪过程中某帧出现问题,利用局部地图,我们仍然可以求出之后那些帧的正确位姿。以下是进行局部地图跟踪的流程:

    (1)UpdateLocalMap()更新局部地图(关键帧UpdateLocalKeyFrames()+局部地图点UpdateLocalPoints());

    更新局部关键帧mvpLocakKeyFrames步骤如下:

    1. 搜索具有共视关系的关键帧。由之前的帧间匹配,我们得到当前帧的地图点(MapPoint),通过遍历这些地图点,得到也能观测到这些地图点的关键帧(地图点上记录了共视信息)。同时记录关键帧的共视次数。关键帧和共视次数存储在map<KeyFrame*,int> keyframeCounter;
    2. 将步骤1得到的关键帧全部插入到mvpLocakKeyFrames
    3. 遍历步骤1keyframeCounter中每个关键帧,得到与之共视程度最高的10个关键帧,也全部插入到mvpLocakKeyFrames;
    4. 遍历步骤1keyframeCounter中每个关键帧,将每个关键帧的子关键帧(>=1)和父关键帧(=1,共视程度最高的之前的关键帧)都插入到mvpLocakKeyFrames ;
    5. 更新当前帧的参考关键帧,与自己共视程度最高的关键帧作为参考关键帧
       

    更新局部地图点mvpLocalMapPoints步骤如下:

    1. 清空mvpLocalMapPointsmvpLocalMapPoints.clear();
    2. 遍历局部关键帧mvpLocakKeyFrames,将局部关键帧的地图点插入到mvpLocalMapPoints

    (2)SearchLocalPoints()匹配局部地图视野范围内的点和当前帧特征点;

    步骤如下:

    1. 遍历当前帧的mvpMapPoints,标记这些MapPoints不参与之后的搜索,因为当前的MapPoints一定在当前帧的视野内。这些MapPoints是在帧间匹配过程中用到的。
    2. 将局部地图点mvpLocalMapPoints投影到当前帧,投影矩阵是通过帧间匹配得到。判断局部地图点是否在当前帧的视野内isInFrustum(视野内,mbTrackInView 为true),只有视野内的地图点才进行投影匹配。判断要求包括:投影点是否在图像内;地图点到相机中心距离是否在尺寸变换范围内;相机中心到地图点向量与地图点平均视角夹角是否小于60°。
    3. 对视野内的地图点通过投影进行特征匹配

            matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);,得到一组3D局部地图点-2D特征点匹配对。

    (3)PoseOptimization()根据新的匹配点重新优化相机位姿。

    总结:可以看出,这几步和TrackWithMotionModel类似,只不过TrackWithMotionModel匹配点对是通过帧间匹配得到的,而TrackLocalMap是剔除TrackWithMotionModel中的匹配点,从局部地图中找新的更多的匹配点,从而实现对位姿的进一步优化。
     

    展开全文
  • clear all; R=2; N=1000; v=normrnd(0,sqrt(R),[1,N]); k=[0:N-1]; dt=0.1; vel_theory=10;%理论速度 pos_theory=12+k*vel_theory*dt; pos_ob=pos_theory+v; vel_pred=linspace(0,0,N);...0.01 0.0
    clear all;
    R=2;
    N=1000;
    v=normrnd(0,sqrt(R),[1,N]);
    k=[0:N-1];
    dt=0.1;
    vel_theory=10;%理论速度
    pos_theory=12+k*vel_theory*dt;
    pos_ob=pos_theory+v;
    vel_pred=linspace(0,0,N);%预测的速度
    pos_pred=linspace(0,0,N);
    A=[1 dt;0 1];
    H=[1 0];
    xk=[0 0]';
    P=[0.01 0.01;0.01 0.001];
    Q=[0.01 0;0 0.01];
    for i=1:1:N
        xk=A*xk;
        P=A*P*A'+Q;
        Kg=P*H'/(H*P*H'+R);
        xk=xk+Kg*(pos_ob(i)-H*xk);
        pos_pred(i)=xk(1);
        vel_pred(i)=xk(2);
    end
    plot(vel_pred,'r')
    

    在这里插入图片描述

    展开全文
  • 匀速直线运动模糊图像的退化数学模型试验研究
  • 分析了目标在该系统成像的运动特性,结合系统帧频采样率较高的特点,提出了一种改进的匀速运动模型,并从理论上与“当前”统计模型的跟踪精度进行了对比分析。结果表明,在红外鱼眼系统采样率较高的情况下,匀速运动...
  • 针对背景不变的局部匀速运动模糊图像复原问题, 提出一种基于Z变换的恢复算法。在前景和背景色差较大的假设前提下, 基于背景差方法将前景从图像中分离出来; 利用旋转矩阵将前景在像平面内任意方向的运动转换成X轴方向...
  • 针对超声波相位法测距应用中测距端运动所造成的多普勒效应对测距超声波传播频率的影响,提出了改进的超声波相位法测距数学模型,其中重新推导了超声波总相位差与空间距离的关系;为了适应普遍情况还对变速运动情况下...
  • 等速模型是用于物体跟踪的最基本的运动模型之一。 四个状态变量依次为横坐标,纵坐标,与x轴夹角(逆时针为正),线速度 1.1 状态转移方程 1.2 局限 等速(CV)模型(速度是常量的模型)的局限性: 假设速度是常量...

    修订

    • 20200324 CTRV模型

    一次运动模型(也别称为线性运动模型)

    • 链接:https://blog.csdn.net/AdamShan/article/details/78265754

    恒定速度模型(Constant Velocity, CV)
    恒定加速度模型(Constant Acceleration, CA)
    这些线性运动模型假定目标是直线运动的,并不考虑物体的转弯。

    1.0 匀速模型(Constant Velocity, CV)

    在扩展卡尔曼滤波课程中,我们使用了一个等速模型(CV)。等速模型是用于物体跟踪的最基本的运动模型之一。

    版本一

    状态空间可以表示为:
    在这里插入图片描述

    转移函数为:

    在这里插入图片描述

    版本二:

    在这里插入图片描述
    四个状态变量依次为横坐标,纵坐标,与x轴夹角(逆时针为正),线速度

    1.1 状态转移方程

    在这里插入图片描述

    1.2 局限

    等速(CV)模型(速度是常量的模型)的局限性:
    假设速度是常量,我们实际上简化了车辆实际移动的形式,因为大多数车辆道路是有拐弯的,但速度是常量的模型会无法正确预测拐弯车辆。

    2.0 匀加速模型(Constant Acceleration, CA)

    在这里插入图片描述
    四个状态变量依次为横坐标,纵坐标,与x轴夹角(逆时针为正),线速度,加速度

    2.1 状态转移方程

    在这里插入图片描述

    二次运动模型:

    • 恒定转率和速度模型(Constant Turn Rate and Velocity,CTRV)

    • 恒定转率和加速度模型(Constant Turn Rate and Acceleration,CTRA)
      CTRV目前多用于机载追踪系统(飞机),这些二次运动模型大多假定速度 v和 偏航角速度(yaw rate)ω 没有关系,因此,在这类运动模型中,由于**偏航角速度ω **测量的扰动(不稳定),即使车辆没有移动,我们的运动模型下的角速度也会发生细微的变化。

    • 为了解决这个问题,速度 v 和 偏航角速度 ω 的关联可以通过设定转向角 Φ 恒定来建立,这样就引出了 恒定转向角和速度模型(Constant Steering Angle and Velocity,CSAV), 另外,速度可以别假定为线性变化的,进而引出了常曲率和加速度模型(Constant Curvature and Acceleration,CCA)。

    • 论文参考:Empirical evaluation of vehicular models for ego motion estimation

    在这里插入图片描述
    图片:论文:Empirical evaluation of vehicular models for ego motion estimation

    1.0 恒定转弯率和速度幅度模型(Constant Turn Rate and Velocity, CTRV)

    CTRV实际上是CV的一般形式,当角速度=0时(这里需要用到洛必达法则),就是CV的形式。CTRV模型假设对象沿直线前进,同时还能以固定的转弯速率和恒定的速度大小移动。可以看做一段圆弧运动
    四个状态变量依次为横坐标,纵坐标,速度大小用v表示;第四个参数是偏航角,表示方向;第五个参数是估算的角速度。
    在这里插入图片描述

    在这里插入图片描述
    一个恒定的转弯率,就像在ctrv模型中那样,角速度的变化率是0
    在这里插入图片描述

    假设离散的时间步骤kk和持续的时间值tkt_k相关,离散的时间步骤k+1k+1和持续的时间值tk+1t_{k+1}相关,tk+1t_{k+1}tkt_k之间的时间差叫做δt\delta t.

    在这里插入图片描述
    对于上图中求积分解得到:
    在这里插入图片描述

    模型的一个问题:角速度ω=0(ω=ψ^ω=\hat{\psi}是角速度)

    使用CTRV还存在一个问题,那就是 ω=0 的情况,此时我们的状态转移函数公式中的 (x,y) 将变成无穷大。为了解决这个问题,我们考察一下 ω=0 的情况,此时我们追踪的车辆实际上是直线行驶的,所以我们的 (x,y) 的计算公式就变成了:

    x(t+Δt)=vcos(ψ)Δt+x(t)x(t+Δt)=vcos(\psi )Δt+x(t)
    y(t+Δt)=vsin(ψ)Δt+y(t)y(t+Δt)=vsin(\psi )Δt+y(t)

    注意:ψ\psi是值得角度,ψ^\hat{\psi}是角速度,ψ^^\hat{\hat{\psi}}角加速度
    角速度为0时:
    在这里插入图片描述

    CTRV过程噪声
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    1.1 该模型运用EKF的例子

    • 链接:https://blog.csdn.net/AdamShan/article/details/78265754

    在这里插入图片描述
    其中,θ 为偏航角,是追踪的目标车辆在当前车辆坐标系下与x轴的夹角,逆时针方向为正,取值范围是[0, 2π), ω是偏航角速度。
    在这里插入图片描述
    由于EKF采用:
    在这里插入图片描述

    在这里插入图片描述
    在我们后面的Python实现中,我们将使用numdifftools包直接计算雅可比矩阵,而不需要我们使用代码写这个雅可比矩阵。在得到我们CTRV模型的雅可比矩阵以后,我们的处理模型就可以写成:
    在这里插入图片描述

    处理噪声

    处理噪声模拟了运动模型中的扰动,我们引入运动模型的出发点就是要简化我们要处理的问题,这个简化是建立在多个假设的基础上(在CTRV中,这些假设就是恒定偏航角速度和速度),但是在现实问题中这些假设就会带来一定的误差,处理噪声实际上描述了当我们的系统在运行一个指定时间段以后可能面临多大的这样的误差。

    在CTRV模型中噪声的引入主要来源于两处:直线加速度和偏航角加速度噪声,我们假定直线加速度和偏航角加速度满足均值为 0,方差分别为 σa2,σω˙2σ^2_a, σ^2_{ω˙}的高斯分布,由于均值为 0, 我们在状态转移公式中的 u就可以不予考虑,我们来看噪声带来的不确定性 Q ,直线加速度和偏航角加速度将影响我们的状态量(x,y,v,θ,ω)(x,y,v,θ,ω) ,这两个加速度量对我们的状态的影响的表达式如下:
    在这里插入图片描述

    其中 μa,μω˙μ_a,μ_{ω˙} 为直线上和转角上的加速度(在这个模型中,我们把把它们看作处理噪声),我们分解这个矩阵:

    在这里插入图片描述
    在这里插入图片描述

    测量

    激光雷达:测量目标车辆的坐标 (x,y) 。这里的x,y是相对于我们的车辆的坐标系的,即我们的车辆为坐标系的原点,我们的车头为x轴,车的左侧为y轴。
    雷达:测量目标车辆在我们车辆坐标系下与本车的距离 ρ,目标车辆与x轴的夹角 ψ,以及目标车辆与我们自己的相对距离变化率 ρ˙(本质上就是目标车辆的实际速度在我们和目标车辆的连线上的分量)

    激光雷达的测量模型仍然是线性的,其测量矩阵为:
    在这里插入图片描述

    雷达的预测映射到测量空间是非线性的,其表达式为:
    在这里插入图片描述
    虽然这个雅可比矩阵看似非常复杂,但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式,在本篇中,我们采用numdifftools这个公式来直接求解雅可比矩阵

    代码:
    请移步至:https://blog.csdn.net/AdamShan/article/details/78265754

    UKE实例

    • https://blog.csdn.net/AdamShan/article/details/78359048

    1.0 非线性处理/测量模型

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

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

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

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

    2.0 无损变换

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

    有点粒子滤波的意思,可以移步至https://blog.csdn.net/djfjkj52/article/details/104562430,了解例子滤波的理解。

    在这里插入图片描述
    在这里插入图片描述

    • 计算一组sigma点
    • 每个sigma点都有权重
    • 通过非线性函数变换点
    • 根据加权点计算高斯

    预测

    χχ——Chi,是第二十二个希腊字母。 χ的ch是一个清软颚擦音
    λλ——Lambda,是第十一个希腊字母。 大写Λ用于: 粒子物理学上,Λ重子的符号

    由一个高斯分布产生sigma point

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

    其中的 λ 是一个超参数,根据公式,λ 越大, sigma点就越远离状态的均值,λ 越小, sigma点就越靠近状态的均值。

    在这里插入图片描述

    需要注意的是,在我们的CTRV模型中,状态数量 n 除了要包含5个状态以外,还要包含处理噪声 μa 和 μω˙,因为这些处理噪声对模型也有着非线性的影响。在增加了处理噪声的影响以后,我们的不确定性矩阵 P 就变成了:

    在这里插入图片描述

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

    在这里插入图片描述
    在这里插入图片描述

    以上公式中还存在一个问题,那就是矩阵开平方根怎么计算的问题?
    需要求解:
    在这里插入图片描述
    其中,可得:
    在这里插入图片描述

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

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

    预测sigma point

    现在我们有sigma点集,我们就用非线性函数 g() 来进行预测:
    在这里插入图片描述

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

    在这里插入图片描述

    预测均值和方差

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

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    然后基于每个sigma点的权重去求新的分布的均值和方差:

    在这里插入图片描述

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

    测量更新

    预测测量(将先验映射到测量空间然后算出均值和方差)
    假设:测量更新分为两个部分,LIDAR测量和RADAR测量,其中LIDAR测量模型本身就是线性的,所以我们重点还是放在RADAR测量模型的处理上面,RADAR的测量映射函数为:
    在这里插入图片描述
    这也是个非线性函数,我们用 h() 来表示它,再一次,我们使用无损转换来解决,但是这里,我们可以不用再产生sigma points了,我们可以直接使用预测出来的sigma点集,并且可以忽略掉处理噪声部分。那么对先验的非线性映射就可以表示为如下的sigma point预测(即预测非线性变换以后的均值和协方差):
    在这里插入图片描述
    和前面的文章一样,这里的 R 也是测量噪声,在这里我们直接将测量噪声的协方差加到测量协方差上是因为该噪声对系统没有非线性影响。在本例中,以RADAR的测量为例,那么测量噪声R为:
    在这里插入图片描述

    更新状态

    首先计算出sigma点集在状态空间和测量空间的互相关函数,计算公式如下
    在这里插入图片描述

    计算卡尔曼增益:
    在这里插入图片描述

    更新状态(也就是作出最后的状态估计):
    在这里插入图片描述
    其中 zk+1z_{k+1}是新得到的测量,而 zk+1kz_{k+1|k} 则是我们根据先验计算出来的在测量空间的测量。

    更新状态协方差矩阵:
    在这里插入图片描述

    伪代码分析:

    预测:

    预测的值,用点集合替代;协方差矩阵:运动传播
    在这里插入图片描述

    更新

    在这里插入图片描述

    UKF=UT+KF

    • https://blog.csdn.net/weixin_42647783/article/details/89065436?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

    UKF=UT+KF,算法的实现分成两步走:

    (1)状态的时间更新

    选定状态的个Sigma点;

    利用UT变换计算后验均值和方差;

    (2)状态的观测更新

    利用标准的卡尔曼滤波体系更新,但使用的公式有所差异。

    根据系统的噪声的存在方式,将其分为加性噪声和隐含噪声,对于两种噪声,UKF滤波的处理方式分为两种,分别是简化的UKF和扩维的UKF,这里仍然假设噪声是高斯分布的。

    简化UKF滤波算法(加性噪声)

    可以参考:https://blog.csdn.net/weixin_42647783/article/details/89065436?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

    扩维UKF滤波算法(噪声隐含)

    在这里插入图片描述

    参数估计/学习(之后可以拓展学习)

    没有做参数估计的UKF滤波,Adaptive-UKF在估计误差上与UKF滤波相差不大,但它并不需要指定状态转移噪声和观测噪声的参数,将更有利于在实际中的应用。

    利用EM算法和极大后验概率估计(MAP),对未知的噪声参数做出估计,再利用估计出的参数去递推卡尔曼滤波的解。每做完一轮UKF滤波,就可以更新一次参数,用于下一轮的UKF滤波。

    参考资料:

    pdf:

    • 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

    PPT:

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

    恒定转动率和加速度(CTRA)

    (Constant Turn Rate and Acceleration, CTRA)

    同CTRV相比,转速不变,说明在相同时间内转过的角度是一样的,径向由匀速变成匀加速,那么对应的,位移会变长。
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    恒定转向角和速度(CSAV)

    该CSAV模型网络上的资料不多,后续需要补充

    恒定曲率和加速度(CCA)(Constant Curvature and Acceleration, CCA)

    CTRA中的转速一定改为曲率半径一定,由r=v/ωr=v/ω可知,当曲率半径一定,又要做匀加速运动,那么角速度必然是时变的,因此同样时间内转过的角度必然会增大。CTRV(黑),CTRA(橘),CCA(红)的关系如下图所示

    在这里插入图片描述

    该CCA模型网络上的资料不多,后续需要补充

    展开全文
  • 运动图像复原中,建立图像退化模型的关键是找到准确的点扩展函数(PSF)。提出了一种基于单幅图像的、改进的任意方向匀速直线运动模糊PSF的估计方法。利用基于图像频谱亮线灰度特征的方向鉴别方法鉴别模糊图像的模糊...
  • 起初只是知道Kalman滤波的核心公式和会用,没有仔细研究,最近老师让讲Kalman算法,所以系统的学习了该算法,并结合匀速直线运动模型简单仿真。 1960年R.E Kalman用时域上的状态空间方法提出了Kalman滤波理论,解决...

    Kalman滤波算法详细推导及简单匀速直线运动程序仿真(matlab)

    起初只是知道Kalman滤波的核心公式和会用,没有仔细研究,最近老师让讲Kalman算法,所以系统的学习了该算法,并结合匀速直线运动模型简单仿真。
    1960年R.E Kalman用时域上的状态空间方法提出了Kalman滤波理论,解决了多维非平稳随机信号的滤波问题,解决了时变随机系统滤波问题。广泛用于制导,全球定位,目标跟踪等。
    Kalman滤波器是线性最小方差估计器,也叫最优滤波器。在几何上Kalman滤波估值可以看作是状态变量在由观测生成的线性空间上的射影。
    在详细推导Kalman算法之前我们要先明确几个概念:线性最小方差估计,射影,新息序列
    线性最小方差估计
    射影,新息序列
    Kalman推导
    Kalman推导
    Kalman推导
    Kalman推导
    Kalman推导
    Kalman总结及算法实现方框图
    至此,该算法推导完毕,为了方便就直接截取的我做的PPT,推导过程参照的是邓自立老师著的新息融合滤波理论及其应用
    在推导过程中遇到的疑惑说明一下,希望可以帮到有同样困惑的人。
    线性流形:百度解释:线性流形(linear manifold)是几何学中的常用概念,即Pn中的直线,二维平面,三维平面,…,n-1维平面的统称,个人理解其实可以理解为线性组合

    匀速直线模型
    下面是仿真程序,分为两部分:Kalman函数部分及主函数。
    Kalman函数
    主函数
    主函数
    仿真图:
    仿真图

    展开全文
  • 为了解决刚体动力学模型的复杂性与控制系统的实时性之间的矛盾,对匀速运动条件下 6-UPS型并联机床刚体动力学模型的简化进行了研究。首先采用 Newton-Euler法建立了并联机床的刚体动力学模型,并将模型分解为 15项。...
  • 通过Matlab产生匀速直线运动、匀加速直线运动匀速转弯运动三种运动轨迹的点迹,并加入了杂波。相关参数可自行调整。
  • 研究了曲轴连杆颈非圆磨削的运动模型,针对模型的复杂计算,按照恒磨除率的原理,提出了切点沿连杆颈表面匀速移动的简化计算方法,分析了简化模型对当量磨削厚度的影响,给出了简化模型的修正计算公式。仿真结果表明...
  • 假设俩帧之间是匀速运动,根据这个条件来缩小匹配范围,加速匹配减少计算量。 2、代码部分 //带有运动模型的跟踪 bool Tracking::TrackWithMotionModel() { //匹配点大于90%的总数 ORBmatcher matcher(0.9,true);...
  • 采用细胞自动机方法研究了在单向道路上行驶的2辆不同性能(加速度能力、最大速度等)车辆的运动状态和相互...该模型退化为匀速运动状态情形时,所得结果与近期文献的结果相一致,也证明了文中所用方法的可靠性和合理性。
  • (四)ORBSLAM运动估计

    2019-04-24 12:06:00
    基于匀速运动模型的跟踪; 重定位;  上述三种方案,我们只介绍前两种,重定位由于需要用到回环检测,我们会在之后讲解。 PNP运动估计  在介绍ORBSLAM2的跟踪策略之前,我们先了解...
  • 为解决电磁线圈绕制时易出现的嵌层、塌陷、堆积突起、排线不紧密和不规则等问题,通过分析绕线时层间匀速运动排线和端点折返排线原理,建立了基于轴向压力补偿的层间匀速运动和端点折返运动精密排线数学模型,开发了...
  • 以目标在匀加速及匀速转弯状态下,建立了运动声阵列笛卡尔坐标模型,分析了模型的非线性因素;基于状态空间的变换方法,建立了运动声阵列的修正极坐标模型,为进一步研究运动声阵列跟踪系统的跟踪算法奠定了理论基础...
  • 匀速运动:速度值一直保持不变。 多物体同时运动:将定时器绑设置为对象的一个属性。 注:物体每次运动都应该把之前的定时器清除掉。 二、边界处理 遇到边界是应该停止掉还是反弹,方向相反。 改变物体运动方向...
  • 分别对两种匀速运动情况和一种加速运动进行了计算,由此研究不同车速、匀速和加速运动对接收点处脉动流场和气动噪声的影响.对于匀速运动,车速越大,接收点处流速越大,压力越小,在每个频率上所对应的声压级基本上...
  • Track-association-JPDA

    2015-02-14 14:09:19
    JPDA航迹关联算法,将两个匀速运动模型点迹与航迹进行关联
  • 该算法应用于近程毫米波雷达探测环境下,根据被测目标的实际运动情况建立了匀加速运动、匀速转弯运动及匀速运动模型,在动态规划算法基础上考虑了多种运动模型以及模型之间的转换和预测,避免了因模型不匹配导致的...
  • 3、如果不是第一帧,那么将其与local_map进行ndt配准,配准的所需要的guess_pose是根据匀速运动模型进行计算的。 4、配准得到当前帧的pose,利用当前帧和上一阵的pose,结合匀速运动模型得到下一次配准的guess_pose...
  • 针对传统运动目标参数估计方法将运动目标假设为匀速运动的不足,提出了一种新的多通道运动目标参数...克服了用匀速运动目标模型对加速目标进行运动参数估计误差较大的问题。仿真分析验证了该运动参数估计方法的有效性。
  • KF跟踪时常见的运动模型有匀速运动模型(CV)和匀加速运动模型(CA) 常用传感器是Lidar和Radar 其中,Lidar只能测距不能测速,而radar可以测速。 因此,在使用Lidar进行跟踪时,速度可以初始化为0,在预测阶段...
  • 提出一种基于激光脉冲扫描测量体制下的高速点目标捕获、跟踪测量方法, 应用匀速运动模型模拟仿真目标的轨迹及分布概率; 考虑脉冲激光光斑分布的相关参数, 建立脉冲激光扫描捕获点目标的理论模型。提出了一种基于时间...
  • 应用车辆非匀速行驶时路面随机激励的时域模型,研究了非匀速车辆与弹性支承桥梁耦合系统并建立了其无量纲运动微分方程,获得桥梁跨中的无量纲最大挠度、车辆的舒适度随路面不平度系数、车辆的加速度及前后轮激励...
  • 该方法根据大尺寸回转零件实际运动规律,构建了匀速运动模型和“当前”统计模型,进而给出了基于两种运动模型的交互多模(interacting multiple model,IMM)算法。该方法将多个光栅传感器检测的动态位置信息分别...
  • [笔记]三维激光SLAM学习一(6)——LOAM论文&...之前提到,论文中假设两帧间运动为匀速运动模型,然后使用线性插值计算。也使用这个运动补偿对激光点云进行去畸变。下面我们仔细分析一下。 运动中的载具:

空空如也

空空如也

1 2 3 4 5 ... 8
收藏数 146
精华内容 58
关键字:

匀速运动模型