多传感器融合定位1（激光雷达+毫米波雷达）

前言

LZ最近在看Udacity的无人驾驶课程，该课程主要分为三部分，第一部分的课程主要使用Python实现的车道线识别、车牌识别等计算机视觉项目。由于我对定位、建图等方面有些知识储备，所以先从第二部分课程开始。
本节将用最简洁的话讲解卡尔曼滤波KF、非线性卡尔曼滤波EKF等知识点，并就此实现一个多传感器融合定位的小demo，后面会就粒子滤波PF专门开一个章节讲解。

对于匀速运动模型，KF和EKF的状态预测（Predict）过程是一样的；KF和EKF唯一区别的地方在于测量值更新（Update）这一步。在测量值更新中，KF使用的测量矩阵H是不变的，而EKF的测量矩阵H是Jacobian矩阵。

一、卡尔曼滤波 KF

1、引子

1）两个传感器测量同一个信号，为了减小误差我们可以采用取平均的方式，
2）进一步的我们采用加权平均（由方差大小分配），但加权平均是一种静态分配方式。3）方差是随外界环境而变的，加权值也应该随之改变，这就是卡尔曼滤波出现的原因，它是一种动态更新加权值，不断迭代的算法。

卡尔曼滤波器就是根据上一时刻x(k-1)的状态，预测当前时刻x(k)的状态，将预测的状态x^(k)与当前时刻的测量值z进行加权，加权后的结果才认为是当前的实际状态。

2、对象：线性高斯模型，这个模型最好的地方在于两个高斯分布的乘积仍然是一高斯分布，由此实现模型的动态迭代。

3、本质：参数化的贝叶斯模型。先验估计：对下一时刻系统初步状态估计；结合先验估计以及测量反馈，得到后验估计。

4、应用：最优化自回归数据处理算法、机器人导航、雷达系统、图像处理等

2、黄金公式及推导

直接上结论，下面这个公式分为两部分，上面的是预测，下面是更新。

具体公式推导，我竟然发现了我之前的笔记，下面直接上图解释。
为了避免推导晦涩难懂，采用小汽车的模型进行一步一步的推导。

1、建立模型

2、简单一维小车

假设有个小车匀速运动，我们在左侧安装了一个测量小车距离和速度传感器，每秒测一次小车位置s和速度v。我们用向量$x_{t}$xt​状态向量。

由于测量误差的存在，无法获得真实值，我们可以以一个高斯分布来表示结果在各个地方出现的概率，如下图所示，在真值附近的概率最高。

$x_{t-1}=\begin{bmatrix} s_{t-1}\\ v_{t-1} \end{bmatrix}$xt−1​=[st−1​vt−1​​]
接下来是使用历史信息对未来的位置进行推测，以速度v匀速行走$\Delta t$Δt后，小车位置的红色区域范围变大了，这是因为预测时加入了速度估计的噪声，放大了不确定性。

$\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}$[st​vt​​]=[10​Δt1​][st−1​vt−1​​]
此时传感器对小车做了一次观测,结果为 z

上图蓝色区域为此时观测结果，红色为预测结果，那么最终的结果是怎样的呢？下图绿色部分给出了答案。


我们对预测和观测结果用不同的权重得到最终结果，两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。

3、预测（Predict）

上面是一维小车，状态向量里只有它的位置和速度，当二维小车在平面时它的状态向量有分别是位置和速度的x、y方向（x,y,vx,vy）
$x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix}$x​=⎣⎢⎢⎡​xyvxvy​⎦⎥⎥⎤​

和上面的一维对应
$\begin{bmatrix}s_{t}\\v_{t}\end{bmatrix}= \begin{bmatrix}1 & \Delta t\\ 0& 1\end{bmatrix}\begin{bmatrix}s_{t-1}\\v_{t-1}\end{bmatrix}$[st​vt​​]=[10​Δt1​][st−1​vt−1​​]
这里的二维应该是

$\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}=\begin{bmatrix} 1& 0 & \Delta t& 0\\ 0 & 1 & 0 & \Delta t\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1 \end{bmatrix}\begin{bmatrix}x_{t-1}\\ y_{t-1}\\vx_{t-1}\\vy_{t-1}\end{bmatrix}$⎣⎢⎢⎡​xt​yt​vxt​vyt​​⎦⎥⎥⎤​=⎣⎢⎢⎡​1000​0100​Δt010​0Δt01​⎦⎥⎥⎤​⎣⎢⎢⎡​xt−1​yt−1​vxt−1​vyt−1​​⎦⎥⎥⎤​+Bu
接下来是预测的第二个模块，状态协方差矩阵P和过程噪声Q。P表示系统的不确定程度，卡尔曼滤波器初始化时很大，随着更多数据注入滤波器，不确定度会逐渐变小。Q暂时设为单位矩阵。

$P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix}$P=⎣⎢⎢⎡​1000​0100​001000​000100​⎦⎥⎥⎤​
$Q=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1\end{bmatrix}$Q=⎣⎢⎢⎡​1000​0100​0010​0001​⎦⎥⎥⎤​

4、观测更新（measurement update）

先说第一个公式括号里面的z-Hx，z表示的是测量值，x是4维的状态向量，对于激光雷达测量的z就是位置（x，y），对于毫米波雷达测量的z是位置和角度。需要对状态向量x左乘一个矩阵H，才能将二者映射到同一个空间，直接进行加减运算。
以激光雷达的测量值z为：
$z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}$z=[xm​ym​​]
z-Hx扩展开为：
$\begin{bmatrix}\Delta x\\\Delta y\end{bmatrix}=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}-\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}*\begin{bmatrix}x_{t}\\ y_{t}\\vx_{t}\\vy_{t}\end{bmatrix}$[ΔxΔy​]=[xm​ym​​]−[10​01​00​00​]∗⎣⎢⎢⎡​xt​yt​vxt​vyt​​⎦⎥⎥⎤​
测量矩阵 H 是一个2*4的矩阵
$H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}$H=[10​01​00​00​]

    H_lidar_ = Eigen::MatrixXd(2, 4);
    H_lidar_ << 1, 0, 0, 0,
                0, 1, 0, 0;

接下来说K(z-Hx)中的权重K是如何取的，简单来说是和协方差矩阵相关。

    R_lidar_ = Eigen::MatrixXd(2, 2);
    R_lidar_ << 0.0225, 0,
                0, 0.0225;

这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值,R应该是2*2的矩阵。一般情况下，传感器的厂家会提供该值。

求得K之后，当前时刻的x和P都可以求解出来了，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算。

二、非线性卡尔曼滤波EKF

KF中测量矩阵H是固定的，代码中直接赋值即可。
$H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix}$H=[10​01​00​00​]
但是真实情况下，测量矩阵 H应该是需要求雅克比矩阵求出的。我们接下来以毫米波雷达为例

毫米波原理是多普勒效应，能够测量障碍物在极坐标系下雷达的距离ρ，方向角ϕ和距离的变化率（径向速度）ρ’。

已知量：状态向量x为4*1，

已知量：观测值z的数据维度为3*1.

观测值z和预测值x之间的差值y关系为：


但是如果转化为 y=Hx时，由于第二部分的转化是非线性的，无法找到一个常数矩阵H。

所以我们需要将上述非线性函数转化为近似的线性函数，用一阶泰勒展开。对状态向量x求偏导数，即雅克比Jacobian矩阵。

三、代码讲解

KF和EKF都在kalmanfilter.h/cpp文件中，头文件KalmanFilter类为：

class KalmanFilter {
public:
    // 构造函数和析构函数
    KalmanFilter();
    ~KalmanFilter();
    // 初始化
    void Initialization(Eigen::VectorXd x_in);
    bool IsInitialized();

    // 5个矩阵赋值
    void SetF(Eigen::MatrixXd F_in);
    void SetP(Eigen::MatrixXd P_in);
    void SetQ(Eigen::MatrixXd Q_in);
    void SetH(Eigen::MatrixXd H_in);
    void SetR(Eigen::MatrixXd R_in);
    // 预测
    void Prediction();
    // KF更新
    void KFUpdate(Eigen::VectorXd z);
    // EKF更新
    void EKFUpdate(Eigen::VectorXd z);
    // 获得状态x
    Eigen::VectorXd GetX();

private:
    // Jacobian矩阵
    void CalculateJacobianMatrix();

    // 是否初始化标志
    bool is_initialized_;

    // 状态向量x
    Eigen::VectorXd x_;

    // 状态协方差矩阵P
    Eigen::MatrixXd P_;
    // 状态转移矩阵F  
    Eigen::MatrixXd F_;
    // 过程噪声Q 
    Eigen::MatrixXd Q_;
    // 测量矩阵H 
    Eigen::MatrixXd H_;
    // 测量噪声R 
    Eigen::MatrixXd R_;
};

主要成员变量为：状态转移矩阵F、状态协方差矩阵P、测量矩阵H、过程噪声Q、测量噪声R、状态向量x
主要成员函数为：初始化Initialization()、5个矩阵赋值set()、预测prediction()、KF更新KFUpdate()、EKF更新EKFUpdate().

1、预测Prediction()

// x=F*x
// P=F*P*Ft+Q
void KalmanFilter::Prediction()
{
    x_ = F_ * x_;
    Eigen::MatrixXd Ft = F_.transpose();
    P_ = F_ * P_ * Ft + Q_;
}

2、KF更新KFUpdate()

// x=x+k*(z-H*x)
// P=(I-K*h)*P
// K=P*Ht*(H*P*Ht+R)t
void KalmanFilter::KFUpdate(Eigen::VectorXd z)
{
    Eigen::VectorXd y = z - H_ * x_;
    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

3、EKF更新EKFUpdate()

EKF的预测两个公式和之前一样，都是Prediction()函数，区别在于求解测量矩阵H矩阵。
首先是要求解测量值z和预测值x之间的偏差。y=z-Hx。
这里Hx用的就是将x从笛卡尔坐标系转化为极坐标系。
接下来的主要问题是在求解H矩阵。

// KF和EKF区别在于测量矩阵H的计算
void KalmanFilter::EKFUpdate(Eigen::VectorXd z)
{
    double rho = sqrt(x_(0)*x_(0) + x_(1)*x_(1));
    double theta = atan2(x_(1), x_(0));
    double rho_dot = (x_(0)*x_(2) + x_(1)*x_(3)) / rho;
    Eigen::VectorXd h = Eigen::VectorXd(3);
    h << rho, theta, rho_dot;
    Eigen::VectorXd y = z - h;

    CalculateJacobianMatrix();

    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd S = H_ * P_ * Ht + R_;
    Eigen::MatrixXd Si = S.inverse();
    Eigen::MatrixXd K =  P_ * Ht * Si;
    x_ = x_ + (K * y);
    int x_size = x_.size();
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
    P_ = (I - K * H_) * P_;
}

4、CalculateJacobianMatrix()函数

void KalmanFilter::CalculateJacobianMatrix()
{
    Eigen::MatrixXd Hj(3, 4);

    // get state parameters
    float px = x_(0);
    float py = x_(1);
    float vx = x_(2);
    float vy = x_(3);

    // pre-compute a set of terms to avoid repeated calculation
    float c1 = px * px + py * py;
    float c2 = sqrt(c1);
    float c3 = (c1 * c2);

    // Check division by zero
    if(fabs(c1) < 0.0001){
        H_ = Hj;
        return;
    }

    Hj << (px/c2), (py/c2), 0, 0,
         -(py/c1), (px/c1), 0, 0,
          py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
    H_ = Hj;
}

5、main主函数

主函数基本流程为：

int main()
{
    KalmanFilter kf;
    while(getlint(x,y,时间戳)){
         if(激光雷达尚未初始化){
             kf.Initialization(x_in);
             P<< 4*4;
             Q<< 4*4;
             H<< 2*4;
             R<<2*2;
         }
         获取两帧时间差delta t;
         F<< 4*4;
         kf.Prediction();
         kf.KFUpdate();
         VectorXd x_out=kf.GetX();
    }
}

参考：
https://zhuanlan.zhihu.com/p/45238681