多传感器融合定位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 x_{t} xt状态向量。
由于测量误差的存在，无法获得真实值，我们可以以一个高斯分布来表示结果在各个地方出现的概率，如下图所示，在真值附近的概率最高。
x t − 1 = [ s t − 1 v t − 1 ] x_{t-1}=\begin{bmatrix} s_{t-1}\\ v_{t-1} \end{bmatrix} xt−1=[st−1vt−1]
接下来是使用历史信息对未来的位置进行推测，以速度v匀速行走 Δ t \Delta t Δt后，小车位置的红色区域范围变大了，这是因为预测时加入了速度估计的噪声，放大了不确定性。
[ s t v t ] = [ 1 Δ t 0 1 ] [ s t − 1 v t − 1 ] \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} [stvt]=[10Δt1][st−1vt−1]
此时传感器对小车做了一次观测,结果为 z
上图蓝色区域为此时观测结果，红色为预测结果，那么最终的结果是怎样的呢？下图绿色部分给出了答案。
我们对预测和观测结果用不同的权重得到最终结果，两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。
3、预测（Predict）
上面是一维小车，状态向量里只有它的位置和速度，当二维小车在平面时它的状态向量有分别是位置和速度的x、y方向（x,y,vx,vy）
x = [ x y v x v y ] x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix} x=⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
和上面的一维对应
[ s t v t ] = [ 1 Δ t 0 1 ] [ s t − 1 v t − 1 ] \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} [stvt]=[10Δt1][st−1vt−1]
这里的二维应该是
[ x t y t v x t v y t ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x t − 1 y t − 1 v x t − 1 v y t − 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} ⎣⎢⎢⎡xtytvxtvyt⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xt−1yt−1vxt−1vyt−1⎦⎥⎥⎤+Bu
接下来是预测的第二个模块，状态协方差矩阵P和过程噪声Q。P表示系统的不确定程度，卡尔曼滤波器初始化时很大，随着更多数据注入滤波器，不确定度会逐渐变小。Q暂时设为单位矩阵。
P = [ 1 0 0 0 0 1 0 0 0 0 100 0 0 0 0 100 ] P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix} P=⎣⎢⎢⎡10000100001000000100⎦⎥⎥⎤
Q = [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] Q=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 1 & 0\\ 0 & 0& 0 & 1\end{bmatrix} Q=⎣⎢⎢⎡1000010000100001⎦⎥⎥⎤
4、观测更新（measurement update）
先说第一个公式括号里面的z-Hx，z表示的是测量值，x是4维的状态向量，对于激光雷达测量的z就是位置（x，y），对于毫米波雷达测量的z是位置和角度。需要对状态向量x左乘一个矩阵H，才能将二者映射到同一个空间，直接进行加减运算。
以激光雷达的测量值z为：
z = [ x m y m ] z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix} z=[xmym]
z-Hx扩展开为：
[ Δ x Δ y ] = [ x m y m ] − [ 1 0 0 0 0 1 0 0 ] ∗ [ x t y t v x t v y t ] \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]=[xmym]−[10010000]∗⎣⎢⎢⎡xtytvxtvyt⎦⎥⎥⎤
测量矩阵 H 是一个2*4的矩阵
H = [ 1 0 0 0 0 1 0 0 ] H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix} H=[10010000]
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 = [ 1 0 0 0 0 1 0 0 ] H=\begin{bmatrix} 1& 0& 0 & 0\\ 0& 1 & 0 & 0 \end{bmatrix} H=[10010000]
但是真实情况下，测量矩阵 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