前言

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。我们用向量xtx_{t}xt状态向量。

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

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

[stvt]=[1Δt01][st−1vt−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=[xyvxvy]x_{}=\begin{bmatrix}x\\ y\\vx\\vy\end{bmatrix}x=⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤

和上面的一维对应
[stvt]=[1Δt01][st−1vt−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]
这里的二维应该是

[xtytvxtvyt]=[10Δt0010Δt00100001][xt−1yt−1vxt−1vyt−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=[10000100001000000100]P=\begin{bmatrix} 1& 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0& 100 & 0\\ 0 & 0& 0 & 100 \end{bmatrix}P=⎣⎢⎢⎡10000100001000000100⎦⎥⎥⎤
Q=[1000010000100001]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=[xmym]z=\begin{bmatrix}x_{m}\\y_{m}\end{bmatrix}z=[xmym]
z-Hx扩展开为：
[ΔxΔy]=[xmym]−[10000100]∗[xtytvxtvyt]\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=[10000100]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=[10000100]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);// 获得状态xEigen::VectorXd GetX();private:// Jacobian矩阵void CalculateJacobianMatrix();// 是否初始化标志bool is_initialized_;// 状态向量xEigen::VectorXd x_;// 状态协方差矩阵PEigen::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 parametersfloat px = x_(0);float py = x_(1);float vx = x_(2);float vy = x_(3);// pre-compute a set of terms to avoid repeated calculationfloat c1 = px * px + py * py;float c2 = sqrt(c1);float c3 = (c1 * c2);// Check division by zeroif(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

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

传感器仿真模型一览 | 摄像头激光雷达毫米波雷达
作者 | 黄浴编辑 | 汽车人点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[多传感器融合]技术交流群后台回复[相机标定]获取 ...
发布职位：图森未来TuSimple# 4月份纳斯达克上市base 北上技术路线：激光雷达+毫米波雷达+摄像头 + 高级规划决策 + 高精地图前端后端软件大数据感知定位算法
发布职位:图森未来TuSimple# 4月份纳斯达克上市 base 北上技术路线:激光雷达+毫米波雷达+摄像头 + 高级规划决策 + 高精地图前端后端软件大数据感知定位算法
【多传感器融合定位】【从零开始做自动驾驶定位_任佬】【所学到的东西汇总】
[多传感器融合定位][从零开始做自动驾驶定位_任佬][所学到的东西汇总] 0 前言 1 开篇 1.1 代码工程的运行 2 数据集 3 软件框架 3.1 运行 3.2 学到的 3.2.1 对消息的订阅和 ...
深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
多传感器融合定位四-3D激光里程计其四：点云线面特征提取
多传感器融合定位四-3D激光里程计其四:点云线面特征提取 1. 点云线面特征提取 1.1 按线数分割 1.2 计算曲率(重要!) 1.3 按曲率大小筛选特征点 2. 基于线面特征的位姿变化 2.1 帧 ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...
多传感器融合定位十四-基于图优化的定位方法
多传感器融合定位十四-基于图优化的定位方法 1. 基于图优化的定位简介 1.1 核心思路 1.2 定位流程 2. 边缘化原理及应用 2.1 边缘化原理 2.2 从滤波角度理解边缘化 3. 基于kitt ...
多传感器融合定位十五-多传感器时空标定(综述)
多传感器融合定位十五-多传感器时空标定 1. 多传感器标定简介 1.1 标定内容及方法 1.2 讲解思路 2. 内参标定 2.1 雷达内参标定 2.2 IMU内参标定 2.3 编码器内参标定 2.4 ...

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

前言