卡尔曼滤波器之无损卡尔曼滤波

参考文献：

【１】https://zhuanlan.zhihu.com/p/35729804

【２】https://blog.csdn.net/l2014010671/article/details/93305871

１．无损卡尔曼滤波简介

        前面已经讲过，经典卡尔曼滤波器可以解决系统状态变量和传感器测量值之间为线性变换的情况，而扩展卡尔曼滤波器为了解决非线性变换的情况，提出通过对非线性函数进行泰勒展开，进行一阶线性化的截断，忽略了其余高阶项，进而完成非线性函数的近似线性化，从而解决了非线性系统下的滤波。但是扩展卡尔曼滤波本身也存在偏导数高阶导数省略问题和雅克比矩阵计算难度的问题，效果不是很好。基于此，无损卡尔曼滤波提出了另外一种解决非线性系统的思路——无损变换！

　　无损变换的主要思想是“近似概率分布要比近似非线性函数更容易”。无损变换计算均值和协方差，通过含有均值和协方差的确定的点集（称作sigma points）来近似概率分布，通过系统的非线性模型，产生繁衍的sigma point，经过选择合适的权值估计均值和协方差。避免了求解雅克比矩阵。这种方法把系统当作“黑盒”来处理，因而不依赖于非线性的具体形式。

　　和ＥＫＦ相比，UKF具有更高的估计精度，并且计算复杂度降低（不需要求雅克比矩阵），满足了具有各种特殊要求的非线性滤波和控制方面的应用，在实现上也比EKF更为简单。

２．无损卡尔曼滤波器

参考文献【１】写的非常清楚了，内容基本一样，主要以目标定位为例，自己写一遍代码加深印象。

ＵＫＦ框架如下：

 代码架构如下：

class UKF
{
public:
    UKF();
    bool init(const UKFParameter& para = UKFParameter());

    bool updateLocation(VectorXd& measure,double timeStamp,VectorXd& state);

private:
    bool isInit = false;
    double _lastTimeStamp = -1;

    UKFParameter _param;

    // UKF
    //MatrixXd _S;           // 测量协方差矩阵
    MatrixXd _R;           // 测量噪声
    MatrixXd _P, _Q;       // 状态方差和噪声
    VectorXd _weights;     // sigma points weights
    MatrixXd _XsigmaPoints; // sigma point (dim_x_aug x 2*dim_x_aug+1)
    MatrixXd _XsigmaPointsPred; // predicted sigma point (dim_x x 2*dim_x_aug+1)
    uint _sigmaPointsNum;   // sigma point number 2*dim_x_aug+1
    VectorXd _X;           // 状态空间

    VectorXd predict(double dt);
    MatrixXd generateSigmaPoints(VectorXd& xAug,MatrixXd& pAug);
    MatrixXd motionProcessCTRV(const double delta_t);

    VectorXd correct(VectorXd &z);

};

typedef struct _Parameter{
    int dim_z       = 4;  // x,y,speed,yaw
    int dim_x       = 5;  // x,y,speed,yaw,yawr
    int dim_x_aug   = 7;  // x,y,speed,yaw,yawr,nu_a,nu_yawdd
    float lambda    = 2;  
    float std_a     = 1.0;
    float std_yawdd = 1.0;
} UKFParameter;

完整代码链接：UKFhttps://download.csdn.net/download/weixin_40059786/85583753