二十二. 智能驾驶之使用无损滤波器(UKF)跟踪感知系统感知的障碍物

        本文涉及到贝叶斯概率(BP),卡尔曼滤波器(KF), 扩展卡尔曼滤波器(EKF)及无损卡尔曼滤波器(UKF)相关知识, 特别本文使用的UKF基于我的博文: <<七.卡尔曼滤波器开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇>>改进而来,对卡尔曼滤波器不熟悉的可以参考我的前7篇卡尔曼滤波器系列博文.

一. 背景介绍

        在实际的智能驾驶环境中，智能驾驶系统不仅要通过各种传感器感知到环境中的物体,识别他们的分类,方位,尺寸,速度,转向角等信息, 还要估计行人或车辆等障碍物的运动状态，这些运动状态显然不能使用简单的线性系统来描述,于是我们可以用非线性系统情况下广泛使用的滤波算法——扩展卡尔曼滤波(Extended Kalman Filter, EKF),或无损卡尔曼滤波器(UKF)来追踪,估计障碍物的运动.

        简单回顾下EKF和UKF的思想要点:

1. UKF是对非线性函数的概率分布(均值μ和方差σ^2)进行近似(sigma points)，用一系列确定的样本来逼近状态的后验概率分布;
2. EKF 是通过偏导数或连续差分,经雅克比矩阵,对非线性函数( f(x) )本身进行近似(线性化)，但是忽略了高阶导数. 

     KF/EKF算法简单易操作，在工业中有广泛的应用。但是它也存在很多缺点：

1. 需要计算非线性模型的雅克比矩阵，计算大，易出错，难得到；
2. 忽略高阶项，估计精度大受影响；
3. 模型不确定性的鲁棒性很差；
4. 在系统达到平稳状态时，将丧失对突变状态的跟踪能力；
5. 如果系统的误差传播函数不能很好的话用线性函数来逼近，可能会导致滤波器发散。
6. 因为偏导数高阶导数省略问题和雅克比矩阵计算难度的问题，让EKF的效果不是很好.

     综上, UKF和EKF计算复杂度相当，但是UKF具有更高的估计精度，满足了具有各种特殊要求的非线性滤波和控制方面的应用，在实现上也比EKF更为简单. 

二. 高级运动模型CTRV(恒定转率和速度模型Constant Turn Rate and Velocity)

       智能汽车领域常用的物体追踪高级运动模型CTRV,即: 恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV), 其状态公式为:

        状态中, 对于3维坐标系中的物体,忽略其z轴(垂直)坐标,只关注其水平面上位置坐标x和y, θ为被追踪的物体(车辆,行人等)相对于当前车辆载体坐标系的X轴的夹角, 角度取值[0,360), 按逆时针方向为正. 即θ为物体偏航角.  ω为物体自身转向角速度,即转率,反应偏航角θ的变化速率.  v就是在偏航角θ方向上物体的速度.

        高级运动模型CTRV的状态模型函数为:

考虑到线加速度α和角加速度β无法准确测量,故将α和β当做系统噪音添加到系统中:

 注: 实际代码实现中, 线加速度α和角加速度β按照一定的正态分布(0,std),当做噪声随机给出.

考虑到ω为0的情况,有:

         当然,如果想更简单, 可以不考虑线加速度α和角加速度β, 完全当系统噪音Q处理. 那样需要合理配置协方差矩阵Q.

三. 无损(或无迹)滤波器UKF

       UKF滤波器算法有两种形式,分别是: UKF滤波算法（加性噪声）版 和 扩维UKF滤波算法（噪声隐含）.

        方法一: 按标准UKF滤波算法（加性噪声）模型实现
        将系统中无法测量和预测的噪音,按照与KF,EKF类似的方式,以相加的方式添加进系统. 如KF和EKF中以加法行使添加B_k,Q_k;
        方法二: 按增广UKF滤波算法（噪声隐含）
        将系统中无法测量和预测的噪音,按照作为系统状态看待,放进系统状态向量X_k,P_k中, 扩张系统状态维度,以状态向量的方式参与系统状态与更新;

        本例,从简单和执行速度考虑, 按标准UKF滤波算法（加性噪声）模型实现.

        在我的<<七.卡尔曼滤波器开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇>>一文中,我实现了一个简单的,非通用的UKF类. 本案例基于此文的UKF类做了做了扩展,实现了一个更通用的UKF类: CTRV_UKF.

        首先,定义一个通用的UKF基类: GenericUKF.

#include <bfl/wrappers/matrix/matrix_wrapper.h>

using namespace MatrixWrapper;

/*
Note： 本例基于简化UKF滤波算法（加性噪声）模型实现
另有： 扩维UKF滤波算法（噪声隐含）   大致算法类似
*/

/*
UKF参数：
    a的取值一般在[1e−4,1)区间内，β为状态分布参数，高斯分布的变量，β=2时最优，如果状态变量是标量的话，则β=0最优。
        λ = α^2 * (n+k) - n           //n为状态向量维数，或状态值个数
详见： https://blog.csdn.net/weixin_44060636/article/details/108150848%CE%B8
*/
#define KK (3) // 在UKF公式中： k为第二个尺度参数，通常设置为3或3-n。  n为状态向量维数，或状态值个数
#define ALPHA (1e-3)  //a的取值一般在[1e−4,1)区间内
#define BETA (0)     //如果状态变量是标量的话，则β=0最优

class GenericUKF
{
private:
    int stateCount = 1;
    int measurementCount = 1;
    int interval = 10; // 预测更新时间间隔,单位 ms

    double lambda = 0;         //此处仅为默认值，具体由UKF参数公式计算
    double lambda_plue_n_x_sqrt = 0; //√(λ+n)   一个常用中间值
    int sigmaPointsCount = 0; // Sigma points count
    ColumnVector weightVector; //保存各个Sigma points的权值向量
    double weightforVariance4PointOne; //对于第一个sigma points点,求其方差时,使用

    ColumnVector X_k; //当前时刻k的先验估计状态向量:  n个元素列向量， 其实就是预测和更新过程的中间状态值
    ColumnVector X_post; //当前时刻后验估计： 最优状态估计值： n个元素列向量

    SymmetricMatrix P_k; //当前时刻k的先验估计 预测协方差矩阵:  其实就是预测和更新过程的中间状态值
    //UKF中P_post根据sigma points计算出来的， 故没有初始值
    SymmetricMatrix P_post;    //最优P_k: 当前时刻最优估计协方差矩阵

    SymmetricMatrix F_k; //系统状态转移矩阵nxn。  此值依赖Δt，故是动态计算
    ColumnVector B_k;    //系统状态控制向量（矩阵）nx1。  此值依赖Δt，故是动态计算

    SymmetricMatrix Q_k; //系统预测过程噪声协方差矩阵nxn

    //传感器自身测量噪声带来的不确定性协方差矩阵
    SymmetricMatrix R_k;

    //Identity matrix(单位矩阵)： 单位矩阵I, 矩阵运算中当数字1使用
    SymmetricMatrix Identity;

public:
    GenericUKF(int _stateCount, int _measurementCount);
    ~GenericUKF();

    void SetInitState(ColumnVector &x);
        
    virtual void UpdateFK(double deltaT, SymmetricMatrix &F_k)=0;
    virtual void UpdateBK(double deltaT, ColumnVector &B_k)=0;
    virtual void TransformToMeasurement(Matrix &xsig_pred, Matrix &zsig_pred, int currentCol)=0;

    void UpdateQK(SymmetricMatrix &q);
        
    //根据使用的多个传感器本身的噪音尺度,更新传感器噪音协方差矩阵
    void UpdateRK(SymmetricMatrix &r);
        
    //传入调用时间间隔Δt和多个传感器测量值.
    void PredictAndUpdate(double deltaT, ColumnVector &z /*in*/);

    //获取最优估计值
    void GetPostEstimate(ColumnVector &x_post /*out*/);//, SymmetricMatrix &p_post /*out*/);

private:
    void Step1_GenerateSigmaPoints(ColumnVector &x /*X_post*/, SymmetricMatrix &P /*P_post*/, Matrix &Xsig);
    void Step2_PredictSigmaPointsByStateFunction(Matrix &Xsig, Matrix &Xsig_pred, double deltaT);
    void Step3_CalculateMeanAndCovarianceByWeight(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k);
    void Step4_PredictSigmaPointsByMeasurementFunction(Matrix &xsig_pred, Matrix &zsig_pred);
    void Step5_CalculateMeasurementMeanAndCovarianceByWeight(Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k);
    void Step6_UpdatePostState(Matrix &xsig_pred, ColumnVector &x_k, SymmetricMatrix &p_k,
                                          Matrix &zsig_pred, ColumnVector &zz_k, Matrix &zzp_k,
                                          ColumnVector &z_k, ColumnVector &x_post, SymmetricMatrix &p_post);
};

        如上代码,关于UKF的核心代码并为改动, 只是将一些和系统状态方程相关的接口定义为虚函数,以便子类根据需要去实现这些接口:

virtual void UpdateFK(double deltaT, SymmetricMatrix &F_k)=0;
virtual void UpdateBK(double deltaT, ColumnVector &B_k)=0;
virtual void TransformToMeasurement(Matrix &xsig_pred, Matrix &zsig_pred, int currentCol)=0;

其次,定义了一个针对高级运动模型(CTRV)的实现类: CTRV_UKF.  继承GenericUKF类,其主要就是实现了上述几个虚函数接口:

#include "GenericUKF.h"

namespace ukf
{

class CTRV_UKF: public GenericUKF{
private:
    std::random_device rd1;
    std::default_random_engine rng1;
    std::random_device rd2;
    std::default_random_engine rng2;

    std::normal_distribution<> norm_linear_acceleration;
    std::normal_distribution<> norm_angular_acceleration;
public:
    CTRV_UKF(int _stateCount, int _measurementCount, double linear_acceleration_stddev, double angular_acceleration_stddev);
    ~CTRV_UKF();
    
    virtual void UpdateFK(double deltaT, SymmetricMatrix &F_k);
    virtual void UpdateBK(double deltaT, ColumnVector &B_k);
    virtual void TransformToMeasurement(Matrix &xsig_pred, Matrix &zsig_pred, int currentCol);
};

}

然后, CTRV高级运动模型分析:

/*
系统状态值X: （x， y, v, θ, ω）;
分别代表被追踪物体的位置(x,y), 前向速度, 偏航角θ, 转向速度(转率)ω
因为无法准确测量被追踪物体的加速度α 和 偏航加速率β,一般做噪声处理.
假设α和β符合均值为0,标准差为sigma的正态分布.  这样α和β的值可以从指定分布中随机取出. 
本例,从简单和执行速度考虑, 按标准UKF滤波算法（加性噪声）模型实现,即 将加速度α 和 偏航加速率β
作为系统噪声,以直接'+' B_k和Q_k的形式放进UKF滤波器.

对CTRV模型分析如下:
======================================================================
当转率ω != 0时:
x_k+1 =  x_k  +   0   +   0   +    0    +  [(v_k/ω_k)sin(θ_k + ω_k*Δt) - (v_k/ω_k)sin(θ_k)]     +    0.5*α*cos(θ_k)*(Δt)^2
y_k+1 =   0   +  y_k  +   0   +    0    +  [-(v_k/ω_k)cos(θ_k + ω_k*Δt) + (v_k/ω_k)cos(θ_k)]    +    0.5*α*sin(θ_k)*(Δt)^2
v_k+1 =   0   +   0   +  v_k  +    0    +                        0                              +           α*Δt
θ_k+1 =   0   +   0   +   0   +   θ_k   +                      ω_k*Δt                           +        0.5*β*(Δt)^2
ω_k+1 =   0   +   0   +   0   +    0    +                        ω_k                            +           β*Δt
其： F_k:
|  1  0  0  0  [(v_k/ω_k)sin(θ_k + ω_k*Δt) - (v_k/ω_k)sin(θ_k)]  |
|  0  1  0  0  [-(v_k/ω_k)cos(θ_k + ω_k*Δt) + (v_k/ω_k)cos(θ_k)] |
|  0  0  1  0   0                                                |
|  0  0  0  1   ω_k*Δt                                           |
|  0  0  0  0   1                                                |
列向量B_k:
| 0.5*α*cos(θ_k)*(Δt)^2 |
| 0.5*α*sin(θ_k)*(Δt)^2 |
|        α*Δt           |
|    0.5*β*(Δt)^2       |
|         β*Δt          |
进一步B_k可以分解为 B_k 和 u_k:
| 0.5*cos(θ_k)*(Δt)^2         0            |      
| 0.5*sin(θ_k)*(Δt)^2         0            |      | α |
|        Δt                   0            |  *   | β |
|         0               0.5*(Δt)^2       |      
|         0                   Δt           |


===========================================================================
当转率ω == 0时:
x_k+1 =  x_k  +   0   +   0   +    0    +  v_k*cos(θ_k)*Δt   +    0.5*α*cos(θ_k)*(Δt)^2
y_k+1 =   0   +  y_k  +   0   +    0    +  v_k*sin(θ_k)*Δt   +    0.5*α*sin(θ_k)*(Δt)^2
v_k+1 =   0   +   0   +  v_k  +    0    +         0          +           α*Δt
θ_k+1 =   0   +   0   +   0   +   θ_k   +         0          +        0.5*β*(Δt)^2
ω_k+1 =   0   +   0   +   0   +    0    +         0          +           β*Δt
其： F_k:
|  1  0  0  0  v_k*cos(θ_k)*Δt  |
|  0  1  0  0  v_k*sin(θ_k)*Δt  |
|  0  0  1  0   0               |
|  0  0  0  1   0               |
|  0  0  0  0   0               |
列向量B_k:
| 0.5*α*cos(θ_k)*(Δt)^2 |
| 0.5*α*sin(θ_k)*(Δt)^2 |
|        α*Δt           |
|    0.5*β*(Δt)^2       |
|         β*Δt          |
进一步B_k可以分解为 B_k 和 u_k (同上ω!=0):
| 0.5*cos(θ_k)*(Δt)^2         0            |      
| 0.5*sin(θ_k)*(Δt)^2         0            |      | α |
|        Δt                   0            |  *   | β |
|         0               0.5*(Δt)^2       |      
|         0                   Δt           |
*/

  最后, 基于上述对CTRV模型的分析,对CTRV_UKF类实现如下:

namespace ukf
{

CTRV_UKF::CTRV_UKF(int _stateCount, int _measurementCount, double linear_acceleration_stddev, double angular_acceleration_stddev)
    :GenericUKF(_stateCount,_measurementCount),
    rng1(rd1()),
    rng2(rd2()),
    norm_linear_acceleration(0.0,linear_acceleration_stddev),
    norm_angular_acceleration(0.0,angular_acceleration_stddev)
{

}

CTRV_UKF::~CTRV_UKF()
{
}

//仅动态计算F_k(1, 3)和F_k(2, 3)，其他保持不变
void CTRV_UKF::UpdateFK(double deltaT, SymmetricMatrix &F_k)
{
    ColumnVector statePostEstimate;
    this->GetPostEstimate(statePostEstimate);

    int x = statePostEstimate(1);
    int y = statePostEstimate(2);
    int v = statePostEstimate(3);
    int theta = statePostEstimate(4);
    int omega = statePostEstimate(5);

    if( theta < 0 )
        theta = 360 + theta;

    if( std::abs( omega ) > 0.0001 ){ // ω_k != 0
/*
其： F_k:
|  1  0  0  0  [(v_k/ω_k)sin(θ_k + ω_k*Δt) - (v_k/ω_k)sin(θ_k)]  |
|  0  1  0  0  [-(v_k/ω_k)cos(θ_k + ω_k*Δt) + (v_k/ω_k)cos(θ_k)] |
|  0  0  1  0   0                                                |
|  0  0  0  1   ω_k*Δt                                           |
|  0  0  0  0   1                                                |
*/      
        float v_o = v/omega;
        F_k(1, 5) = v_o*std::sin(theta+omega*deltaT) - v_o*std::sin(theta);
        F_k(2, 5) = -v_o*std::cos(theta+omega*deltaT) + v_o*std::cos(theta);
        F_k(3, 5) = 0;
        F_k(4, 5) = omega*deltaT;
        F_k(5, 5) = 1;
    }else{  // ω_k == 0
/*
其： F_k:
|  1  0  0  0  v_k*cos(θ_k)*Δt  |
|  0  1  0  0  v_k*sin(θ_k)*Δt  |
|  0  0  1  0   0               |
|  0  0  0  1   0               |
|  0  0  0  0   0               |
*/
        F_k(1, 5) = v*std::cos(theta)*deltaT;
        F_k(2, 5) = v*std::sin(theta)*deltaT;
        F_k(3, 5) = 0;
        F_k(4, 5) = 0;
        F_k(5, 5) = 0;
    }
}

//动态计算B_k
void CTRV_UKF::UpdateBK(double deltaT, ColumnVector &B_k)
{
    ColumnVector statePostEstimate;
    this->GetPostEstimate(statePostEstimate);

    float theta = statePostEstimate(4);

    if( theta < 0 )
        theta = 360 + theta;
        
    float a = norm_linear_acceleration(rng1);  //符合正态分布的 随机 线加速度
    float b = norm_angular_acceleration(rng2); //符合正态分布的 随机 角加速度
/*
列向量B_k:
| 0.5*α*cos(θ_k)*(Δt)^2 |
| 0.5*α*sin(θ_k)*(Δt)^2 |
|        α*Δt           |
|    0.5*β*(Δt)^2       |
|         β*Δt          |
*/
    
    float half_a_TT = 0.5*a*std::pow(deltaT, 2);
    B_k(1) = half_a_TT * std::cos(theta);
    B_k(2) = half_a_TT * std::sin(theta);
    B_k(3) = a*deltaT;
    B_k(4) = 0.5*b*std::pow(deltaT, 2);
    B_k(5) = b*deltaT;
}

void CTRV_UKF::TransformToMeasurement(Matrix &xsig_pred, Matrix &zsig_pred, int currentCol)
{
    //transform 2n+1 sigma points into measurement space
    //z_k(z1,z2,z3,z4,,,,zn)
    // extract values for better readibility
    double x= xsig_pred(1, currentCol);
    double y = xsig_pred(2, currentCol);
    double v = xsig_pred(3, currentCol);
    double theta = xsig_pred(4, currentCol);
    double omega = xsig_pred(5, currentCol);

    // measurement model. 无论线性关系还是非线性关系
    zsig_pred(1, currentCol) = x;
    zsig_pred(2, currentCol) = y;
    zsig_pred(3, currentCol) = v;
    zsig_pred(4, currentCol) = theta;
    zsig_pred(5, currentCol) = omega;
}

}

        写了一个基于CTRV运动模型的UKF测试例,使用椭圆轨迹并添加测量值噪声,简单测试基于CTRV运动模型的CTRV_UKF类:

const static float measureData[][2] = {
    {4.95341,-0.0719022},{4.89413,0.378738},{4.84478,0.739126},{4.94086,0.606701},{4.87404,0.558567},{4.83454,0.697165},{4.65963,0.906869},{4.69936,1.32095},{4.60588,0.861187},{4.48217,1.25706},{4.4235,1.37856},{4.26034,1.55154},{4.19968,1.75138},{4.18449,1.53875},{4.20523,1.59089},{4.12958,1.50197},{4.17046,1.95299},{4.11252,1.43373},{4.10359,1.90568},{4.06334,1.6536},{4.02241,1.64178},{3.97904,1.90279},{3.93561,1.93768},{3.9306,1.85131},{3.85648,1.5049},{3.73603,1.9654},{3.7018,1.97047},{3.64369,1.92426},{3.50058,2.02988},{3.5155,2.05997},{3.4723,2.12487},{3.37935,2.51024},{3.29008,2.39373},{3.22447,2.34589},{3.21425,2.60429},{3.17417,2.14125},{3.12788,2.2073},{3.13279,2.12731},{3.12834,2.26778},{3.02758,2.75064},{2.96758,2.48864},{3.00647,2.63421},{3.00996,2.55361},{3.056,2.71321},{2.93642,2.37906},{2.94663,2.53608},{2.94075,2.42735},{2.86659,2.3295},{2.8417,2.52647},{2.81297,2.25961},{2.78869,2.50491},{2.7077,2.63707},{2.69917,2.5273},{2.62801,2.43132},{2.64342,2.79876},{2.65518,2.5406},{2.49839,2.5479},{2.43352,2.49432},{2.32395,2.79637},{2.30106,2.45541},{2.20522,2.72981},{2.07349,2.95803},{1.94871,2.81851},{1.87229,2.76353},{1.91588,2.64095},{1.91546,2.75092},{1.82158,2.96866},{1.75975,2.66382},{1.72269,2.7977},{1.68201,3.01075},{1.69737,2.4549},{1.72625,2.92978},{1.68812,2.89136},{1.60394,2.78725},{1.55817,3.05832},{1.44721,2.70196},{1.35225,2.77546},{1.37663,3.19769},{1.38058,2.92546},{1.38909,2.79453},{1.45483,2.61185},{1.36735,2.95805},{1.27033,2.52893},{1.20169,2.91119},{1.12424,2.9795},{1.05951,2.77544},{1.08983,3.16893},{1.03067,2.6757},{0.988694,2.80733},{0.890137,3.29429},{0.804977,2.86407},{0.799744,3.47164},{0.617627,2.61769},{0.63436,2.93544},{0.582973,3.25161},{0.55973,2.9031},{0.500594,2.70695},{0.431943,2.41049},{0.400007,2.88512},{0.255248,2.95727},{0.134686,3.14755},{0.0393557,2.96314},{0.0231141,2.86632},{-0.0073712,2.85108},{-0.144551,2.99718},{-0.241071,3.20998},{-0.302374,3.09922},{-0.307793,2.46768},{-0.362978,3.60574},{-0.296287,2.95987},{-0.325457,2.9437},{-0.252862,2.79662},{-0.4234,3.42823},{-0.549063,3.08334},{-0.650027,2.59586},{-0.741806,2.95133},{-0.809335,2.96122},{-0.952604,2.57055},{-1.01217,3.2295},{-1.06789,2.77073},{-1.10937,2.93475},{-1.21736,3.18745},{-1.22168,3.18219},{-1.29479,2.94547},{-1.37155,3.06175},{-1.49708,3.25126},{-1.5176,2.89961},{-1.58405,3.15011},{-1.61269,2.95627},{-1.61088,2.56619},{-1.6499,2.99324},{-1.67163,2.53525},{-1.65094,2.59598},{-1.69175,2.68806},{-1.78265,2.81},{-1.73372,3.00657},{-1.79643,2.78359},{-1.84419,2.82022},{-1.93913,3.06919},{-1.92956,2.76822},{-1.92353,2.82777},{-1.90946,3.04621},{-1.9611,2.45638},{-1.96523,2.69869},{-2.01002,2.64281},{-2.03624,2.58578},{-2.05816,2.74302},{-2.0999,2.48456},{-2.20083,2.73182},{-2.21543,2.72124},{-2.20175,2.77609},{-2.28097,2.98941},{-2.29235,2.72559},{-2.29264,2.98023},{-2.27107,2.78229},{-2.31441,2.69281},{-2.338,2.70884},{-2.36251,2.76441},{-2.37184,2.69305},{-2.41087,2.71714},{-2.40411,2.53341},{-2.49539,2.5795},{-2.5423,2.55432},{-2.5703,2.33409},{-2.60583,2.44846},{-2.61958,2.39626},{-2.6478,2.38623},{-2.7249,2.60371},{-2.77208,2.52911},{-2.84428,2.28296},{-2.95936,2.87565},{-3.05594,2.39104},{-3.13565,2.11821},{-3.23371,2.02751},{-3.22104,2.44286},{-3.26419,2.26064},{-3.27156,2.12179},{-3.33514,2.47983},{-3.48715,2.04035},{-3.57517,2.20174},{-3.59479,2.12837},{-3.65064,1.97906},{-3.66771,1.86602},{-3.58868,2.38023},{-3.53421,2.03091},{-3.59676,2.15443},{-3.62692,2.28228},{-3.56737,2.27922},{-3.63248,2.13598},{-3.70683,2.0788},{-3.72087,2.26515},{-3.74979,1.98615},{-3.86056,1.92606},{-3.90512,1.79947},{-3.98471,1.88381},{-4.04626,1.91032},{-4.05107,1.43283},{-4.15938,1.75102},{-4.15154,1.78469},{-4.14984,1.50628},{-4.2051,1.82086},{-4.29046,1.69002},{-4.31565,1.90928},{-4.42177,1.53372},{-4.56364,1.37814},{-4.5915,1.21773},{-4.68,1.1612},{-4.7425,0.908839},{-4.78449,1.19531},{-4.87505,0.697215},{-4.86453,0.53447},{-4.91824,0.697059},{-4.95036,0.308403},
{-5.1572,-0.074515},{-5.1137,-0.0356047},{-5.00679,0.286727},{-4.94494,-0.323105},{-4.92241,-0.78088},{-4.9658,-0.787791},{-4.90773,-0.284364},{-4.95679,-0.988601},{-4.8423,-0.0268974},{-4.82402,-0.634919},{-4.8117,-0.975593},{-4.86013,-0.745231},{-4.87858,-1.05742},{-4.79842,-0.286645},{-4.86774,-1.06201},{-4.84243,-0.805966},{-4.66168,-0.820102},{-4.55139,-1.20653},{-4.51636,-1.15308},{-4.44096,-1.29365},{-4.42044,-1.62314},{-4.32182,-1.40367},{-4.26144,-1.46404},{-4.1677,-1.80398},{-4.15975,-1.63497},{-4.10313,-1.29624},{-4.11955,-1.82172},{-4.13238,-1.60639},{-4.07104,-2.19389},{-4.00982,-1.76279},{-4.01025,-1.74044},{-3.99169,-2.03866},{-4.00251,-2.03125},{-3.94659,-1.93535},{-3.81385,-2.14155},{-3.76104,-1.93899},{-3.76329,-1.71241},{-3.69771,-1.68013},{-3.63607,-2.10253},{-3.47839,-1.99711},{-3.42747,-2.15263},{-3.4321,-2.26101},{-3.32858,-1.97747},{-3.26893,-1.93656},{-3.219,-2.26959},{-3.14532,-2.42778},{-3.10958,-2.32848},{-2.98457,-2.72624},{-3.00963,-2.37096},{-3.00019,-2.16167},{-3.09683,-2.46408},{-3.07972,-2.20848},{-3.04533,-2.53361},{-2.96961,-2.02165},{-2.94998,-2.87219},{-2.97622,-2.63196},{-2.88793,-2.18375},{-2.91953,-2.54554},{-2.81106,-2.73052},{-2.66441,-2.71271},{-2.56511,-2.37504},{-2.50609,-2.53663},{-2.45041,-2.42593},{-2.42603,-2.67939},{-2.3155,-2.56309},{-2.19037,-2.92988},{-2.19357,-2.56785},{-2.19343,-2.79159},{-2.22477,-2.76447},{-2.13459,-2.31986},{-2.09681,-3.00804},{-2.01738,-2.45428},{-1.97968,-2.90587},{-1.89393,-2.53448},{-1.85563,-2.90121},{-1.86406,-2.424},{-1.759,-2.75405},{-1.764,-3.32272},{-1.77942,-2.99333},{-1.78447,-2.80298},{-1.70289,-2.8614},{-1.67976,-2.4761},{-1.68131,-2.83723},{-1.64784,-2.83412},{-1.65545,-3.13226},{-1.60432,-2.70688},{-1.61067,-2.75063},{-1.60005,-2.99488},{-1.45119,-2.76123},{-1.45271,-2.72143},{-1.38319,-2.86836},{-1.37397,-2.86321},{-1.32639,-2.72143},{-1.36653,-2.79343},{-1.37384,-3.14112},{-1.30751,-2.80948},{-1.20374,-2.86436},{-1.18868,-3.15195},{-1.12183,-2.96058},{-1.11738,-2.67621},{-1.07793,-2.72231},{-1.04215,-2.96334},{-0.959055,-2.79263},{-0.986919,-3.13879},{-0.968776,-2.81855},{-0.950691,-2.98903},{-0.940755,-3.08663},{-0.852404,-2.71027},{-0.839691,-2.78187},{-0.839198,-3.00459},{-0.797371,-3.16945},{-0.774208,-2.96471},{-0.729642,-3.03864},{-0.664075,-3.23071},{-0.596728,-2.68824},{-0.544166,-3.08305},{-0.453588,-2.68573},{-0.390333,-2.84505},{-0.318359,-3.35399},{-0.269907,-2.71928},{-0.176599,-3.08866},{-0.130529,-2.80066},{-0.0893727,-2.86477},{0.00718187,-3.25157},{0.0293996,-3.00076},{0.0489924,-3.11014},{0.0665742,-3.13467},{0.156284,-2.60262},{0.154009,-3.00827},{0.216116,-2.69933},{0.259936,-3.02528},{0.240758,-2.95555},{0.270774,-2.86993},{0.341541,-3.05131},{0.324392,-2.92892},{0.447627,-2.77733},{0.548615,-3.10482},{0.617985,-2.9281},{0.601067,-3.01252},{0.632664,-3.39313},{0.761523,-2.83387},{0.749736,-2.8263},{0.76459,-3.06716},{0.752222,-3.06872},{0.72368,-3.22264},{0.843831,-3.07787},{0.856768,-2.97697},{0.951411,-3.09655},{0.944506,-3.19416},{1.10118,-2.68176},{1.1486,-2.9505},{1.20293,-2.8872},{1.30624,-2.83555},{1.35891,-2.84564},{1.35699,-2.90746},{1.47526,-3.20676},{1.51637,-2.69687},{1.57997,-2.71276},{1.67806,-2.60393},{1.72927,-3.19419},{1.85878,-2.93116},{1.83856,-2.59436},{1.89786,-3.06933},{1.92718,-2.77514},{1.93143,-3.0184},{1.99006,-2.79065},{2.05023,-2.89057},{2.08942,-2.74655},{2.07926,-3.18002},{2.11825,-2.51329},{2.17359,-2.70637},{2.3101,-2.69063},{2.3322,-2.7506},{2.37902,-2.52935},{2.41189,-2.35844},{2.46207,-3.18834},{2.51186,-2.88977},{2.5706,-2.41191},{2.52631,-2.65365},{2.6011,-2.72977},{2.57521,-2.79733},{2.7131,-2.54083},{2.73464,-2.58471},{2.89779,-2.65812},{2.90403,-2.23178},{2.88545,-2.5193},{2.91058,-2.54767},{2.94367,-2.44285},{2.94641,-2.35877},{2.96008,-2.3079},{3.0342,-2.50489},{3.13403,-2.30501},{3.18541,-2.30097},{3.21939,-1.84091},{3.33297,-2.26703},{3.26891,-2.05033},{3.29298,-2.06841},{3.2819,-2.16802},{3.35739,-2.39938},{3.43773,-2.08542},{3.42659,-1.96611},{3.38555,-2.23114},{3.47747,-2.21856},{3.49628,-2.48003},{3.59292,-1.90758},{3.63969,-2.2158},{3.68127,-2.2167},{3.73572,-1.86943},{3.80038,-1.80966},{3.8413,-2.04883},{3.84252,-2.18196},{3.88373,-1.69715},{3.87717,-1.92775},{3.88385,-2.02882},{3.945,-1.72442},{4.06607,-1.76358},{4.06386,-1.70896},{4.06297,-1.69424},{4.0719,-1.61283},{4.05418,-1.59319},{4.03043,-1.88973},{4.10855,-1.67909},{4.1091,-1.78839},{4.21451,-1.43994},{4.22902,-1.87663},{4.32765,-1.47903},{4.33116,-1.49143},{4.33742,-1.32485},{4.32036,-1.45867},{4.25241,-1.47626},{4.33808,-1.59844},{4.35885,-1.43987},{4.30264,-1.36254},{4.34502,-1.56269},{4.34032,-1.21481},{4.44853,-1.37956},{4.55826,-1.28694},{4.54162,-1.35629},{4.57295,-1.66531},{4.59613,-1.18668},{4.64791,-0.798097},{4.67225,-0.943191},{4.79079,-1.16226},{4.80248,-0.555752},{4.83003,-0.904314},{4.87304,-0.591207},{4.90629,-0.869536},{5.00353,-0.20964}
 };

const static int DATA_COUNT = 461;

//int main(int argc, char **argv)
int main_test()
{   
    CTRV_UKF ukf(5/*状态值个数*/,5/*测量值个数*/, 0.01/*加速度α标准差*/, 0.01/*角加速度β标准差*/);

    ColumnVector stateX(5); //系统状态值X: （x， y, v, θ, ω）;
    ColumnVector measurementZ(5); //5个测量值
    float deltaT = 1;//s

    //如分析描述，
    stateX(1) = 5.0; // x
    stateX(2) = 0.0001; // y
    stateX(3) = 0.1;// v
    stateX(3) = 0.0;// θ
    stateX(3) = 0.0;// ω
    ukf.SetInitState(stateX);
    
    //保存结果最优估计值X_post数组
    ColumnVector statePostEstimateArray[DATA_COUNT];
    
    SymmetricMatrix Q(5); 
    Q = 0.0;
    Q(1, 1) = 1e-3 + 0.00001; 
    Q(2, 2) = 1e-3 + 0.00001; 
    Q(3, 3) = 1e-3 + 0.00001;
    Q(4, 4) = 1e-3 + 0.00001;
    Q(5, 5) = 1e-3 + 0.00001;

    ukf.UpdateQK(Q);//更新下传感器噪音

    //我根据生成模拟测量值数据时,设置的噪音,大致设置下各传感器噪音的方差(协方差).  
    //实际项目中应从传感器说明手册或厂家获取.
    SymmetricMatrix R(5); 
    R = 0.0;
    R(1, 1) = 0.0025; 
    R(2, 2) = 0.004; 
    R(3, 3) = 0.0000022;
    R(4, 4) = 0.0000022;
    R(5, 5) = 0.0000022;

    ukf.UpdateRK(R);//更新下传感器噪音

    for( int i = 0; i < DATA_COUNT; i++ ) // 模拟预测和更新周期是2， 即每秒2执行2次。  30秒执行60次.  deltaT = 0.5s,当然这里不用真去delay(500)
    {
        measurementZ(1) = measureData[i][0];
        measurementZ(2) = measureData[i][1];
        measurementZ(3) = 0;//measureData[i][2];
        measurementZ(4) = 0;//measureData[i][3];
        measurementZ(5) = 0;//measureData[i][4];

        ukf.PredictAndUpdate(deltaT, measurementZ);

        ukf.GetPostEstimate(statePostEstimateArray[i] /*out*/);
    }
    
    cout << "state=\n{\n";
    for (unsigned int i = 0; i < DATA_COUNT; i++)
    {
        //if( -5 <= statePostEstimateArray[i](1) && statePostEstimateArray[i](1) <= 5 && -3 <= statePostEstimateArray[i](2) && statePostEstimateArray[i](2) <= 3)
            cout << "("<<statePostEstimateArray[i](1) << ","<<statePostEstimateArray[i](2)<< ","<<statePostEstimateArray[i](3)<< ","<<statePostEstimateArray[i](4)<< \
            ","<<statePostEstimateArray[i](5)<<"),";
    }
    cout <<"}"<< endl;
    

    return 0;
}

Plot效果图如下(红色为添加噪声的测量值曲线,蓝色为UKF滤波后轨迹曲线):

四. 详细过程

        应用UKF滤波器进行物体跟踪的前提是:  需要利用车载激光雷达Lidar,微波雷达或固态雷达, 及多方位车载摄像头等传感器, 先经基于点云聚类的障碍物检测或基于点云和视觉融合的障碍物检测,获取检测到的障碍物类别, uuid, 位置,尺寸, 速度,偏航角等信息.  然后才是利用滤波器进行物体跟踪.

       在智能驾驶感知系统,通过多种传感器感知到环境中的非静止障碍物时, 根据其id,位置等系统初始化UKF实例(本文基于测试目的,仅针对单一车辆实例化了一个UKF), 后续按照UKF滤波器持续使用CTRV状态方程更新状态,使用感知系统的测量值矫正预测,最终得到一个最后障碍物状态估计:（x， y, v, θ, ω）.

       一个智能驾驶感知系统发布的障碍物信息结构如下:

uuid:  uuid                              dede81a6-3ca8-475f-bf2d-3af223d6b6ef
label                                                       Pedestrian
yaw                                                           0.705874
stationary                                                       False
camera_used                                                          1
position.x                                                      66.664
position.y                                                      53.928
position.z                                                       0.433
dimensions.x                                                     0.855
dimensions.y                                                     1.375
dimensions.z                                                     1.494
attributes.object_motion                                           NaN
cuboids.sibling_id                59ee5dc3-f62a-4cd8-a1e0-dfbe8bb75692
cuboids.sensor_id                                                    0
attributes.rider_status                                            NaN
attributes.pedestrian_behavior                                Standing
attributes.pedestrian_age                                        Adult
Name: 93, dtype: object

        根据每次预测更新的状态值X_post（x， y, v, θ, ω）,发布一个rviz箭头:visualization_msgs::Marker marker.  预测的后验状态值中使用x,y值更新marker的位置,θ值描述箭头的方向(此箭头只是上述障碍物信息结构中的原始yaw值, 并没有在X状态中预测更新):

void RosEntryClass::PublishTrackedTarget(std_msgs::Header header,float theta)
{
    if( pCtrv_ukf == NULL )
        return;

    ColumnVector statePostEstimate;
    pCtrv_ukf->GetPostEstimate(statePostEstimate /*out*/);

    visualization_msgs::Marker marker;

	marker.header = header;

    marker.type = visualization_msgs::Marker::ARROW;
	marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);

	// Rviz marker
    marker.id = 1;
    marker.color.a = 1.0; //alpha!
    marker.color.r = 1.0; //红色
    marker.color.g = 0.0;
    marker.color.b = 0.0;
	marker.scale.x = 1.0; 
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    geometry_msgs::Point p1, p2; 
     
    p1.x = statePostEstimate(1);
    p1.y = statePostEstimate(2); 
    p1.z = 0; 

	p2.x = p1.x + std::cos(theta)*3; //箭头方向和长度
    p2.y = p1.y + std::sin(theta)*3;
    p2.z = 0;
    marker.points.push_back(p1); 
    marker.points.push_back(p2);

    tracked_target_marker_pub_.publish(marker);
}

五.在智能驾驶点云场景测试运行效果

        为了测试我选中一辆汽车作为跟踪对象, 并录制了屏幕,由于无法上传视频,只好上视频截图,如下(测试车辆在图中网格区中心位置):

图1: 64线360度激光雷达已经检测到车身左后侧有车辆,开始跟踪车辆, 根据测量值获取位置和偏航角信息,但偏航角误差较大, 实现中已备忽略.

图2: 被跟踪车辆持续前进,接近和测试车辆左右并列位置.

图3: 被跟踪车辆越过测试车辆车头位置, 已进入测试车辆前置相机监控范围, 下图左侧相机视角,可见左前方车辆车头部分.