卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波是融合多种传感器数据用于状态估计的王牌算法,在自动驾驶定位领域应用广泛。本章主要利用Udacity的一个小案例来梳理KF/EKF/UKF的基本原理及应用方法。项目主要是利用激光雷达数据和毫米波雷达数据来融合完成实时的目标跟踪。这是在二维上的简单案例,需要估计的系统状态变量只有位置、速度(px py vx vy)
。传感器的观测量,激光雷达测量的位置(px py)
,毫米波测量的是距离、角度和角度变化率(rho phi rho_phi)
。

代码地址:https://github.com/JokerJohn/kalman_filter
简介
数据

激光测量R:【vx, vy】
毫米波测量L:【ro, rhi, r_dot】
当前数据可能是激光,也可能是毫米波,穿插着来。
毫米波数据观测空间->状态空间变换是非线性的,可以采用EKF将转换函数h(x)一阶泰勒展开,近似线性化,从而变成传统的卡尔曼滤波流程。也可以通过取上一时刻观测的sigma点,分别通过h(x)计算转换后的sigma点集状态矩阵,用这些sigma点来近似拟合一个高斯分布,加权均值以及协方差。
激光雷达
激光雷达这里做了简化处理,只测量px和py两个维度。其到系统状态的转换是线性的,很容易写出转换矩阵H。
毫米波雷达
其中毫米波雷达的传感器模型及测量原理可参考博客:
http://xchu.net/2019/09/16/%E6%AF%AB%E7%B1%B3%E6%B3%A2%E9%9B%B7%E8%BE%BE%E5%8E%9F%E7%90%86/
如图所示,毫米波测量数据是基于极坐标系的,那么转换到笛卡尔坐标系即为
px = rho*cos(phi)
py = rho*sin(phi)
vx = rho_dot * cos(phi)
vy = rho_dot * sin(phi)

显然,系统状态量(px py vx vy)到毫米波观测的转换是一种非线性的变换过程,无法直接写成H矩阵,所以这里可以采用EKF先对转换函数进行一阶线性近似,需要计算雅克比。
就个人经验来看,分清楚状态空间和观测空间至关重要,状态空间到观测空间的转换用h(x)表示,其系数即为H矩阵。在卡尔曼滤波的两个关键步骤中,预测是根据上一时刻的系统状态量,预测当前的系统状态量,不涉及到观测空间与状态空间的转换。而在测量更新上,测量的数据是在观测空间下,预测的状态是在状态空间下,这里就需要将预测的结果统一到状态空间。
KF/EKF
预测当前位置和速度,状态变量【px, py, vx, vy】
流程:
- 第一帧数据初始化EKF,包括状态量x_k,协方差矩阵P_k,过程噪声Q和测量噪声R
- 预测:后续每帧数据,根据运动模型计算F矩阵和过程噪声Q(预测当前状态x_k, 不确定性P_k(状态空间)
- 预测状态转换到观测空间(H矩阵)
- 当前观测z和不确定度R,求残差(z-Hx_k),残差不确定度(HQ*H(T)+R),卡尔曼增益K
- 根据卡尔曼增益,计算融合后的状态量x’和不确定度P’
KF/UKF
预测当前位置、速度、偏航角及变化率,状态变量【px, py, v, yaw, yaw_rate】
- 第一帧数据初始化EKF,包括状态量x_k,不确定性P_k,sigma点权重
- 计算sigma点转换后的点集矩阵(维度*点集数目)
- 根据运动模型预测系统状态量(这里用sigma点集矩阵衡量),预测不确定度P(P_K-1+权重维度变化量)
- 计算拟合的高斯参数,根据观测计算残差、残差不确定度,卡尔曼增益
- 根据卡尔曼增益计算融合后的状态量x’和不确定度P’
KF/EKF/UKF概述
对卡尔曼滤波的理解可以参考这篇老外写的很有名的博客:
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
假设传感器数据符合高斯分布,根据运动模型预测系统的下一时刻的状态,这个过程是线性的,那么预测的状态下,也是符合高斯假设的,包括其过程噪声。此时系统的观测数据是符合高斯分布,并且带有一定测量噪声。卡尔曼滤波的递推过程,可以理解成两个高斯分布的融合(乘法),也就是卷积的过程。计算出融合后新的分布即为我们要的结果。

这里按一维的卡尔曼滤波去推导,很容易理解卡尔曼增益K的具体含义。即描述预测的误差占实际误差的比率,这个比率可以作为权重去衡量预测和观测的可信度。

计算出卡尔曼增益之后,可以根据卡尔曼增益作为权重,去更新当前的状态变量,那么更新后的状态变量既考虑了上一时刻的预测值,也考虑当前的观测值,以及系统的噪声。同时更新了系统的不确定度P,用于下一个周期的运算,从而完成一个闭环的过程。
按这个思路可以写出经典的卡尔曼滤波的7个公式

EKF
但是现实世界中,很多时候传感器观测到系统状态的转换并不是线性的,常见的处理方法是通过泰勒公式将h(x)在一阶或者二阶处去近似,而忽略其高阶项,得到近似的线性化方程,用以替代非线性函数h(x)。
参考:https://www.zhihu.com/search?type=content&q=ekf



拓展到多维里面,就需要求各个变量的偏导数,对x求偏导数所对应的这一项被称为雅可比(Jacobian)式,即测量矩阵H。


求得非线性函数h(x’)对px,py,vx,vy的一阶偏导数,并排列成的矩阵,最终得到雅克比(Jacobian)矩阵H:

其中
最终计算的测量矩阵为

UKF
UKF则不是直接去线性化H(x),而是考虑在上一时刻系统的状态变量分布上随机选取有限个sigma代表粒子,通过这个非线性过程可以计算得到预测的每一个粒子的状态,转换到观测空间下,可以拟合出一个高斯分布,将此分布和观测的分布去融合即可。
EKF融合流程
首先对系统状态以及卡尔曼步骤进行初步分析,可以写出F和H矩阵。
系统状态量:px, py, vx, vy
观测量:
1.激光雷达:px, py ,可直接作为系统状态量使用, 无需转换(H矩阵)
2.毫米波雷达:rho(距离), phi(角度), rho_dot(角度变化率), 需要先转换成系统状态量使用(非线性过程)
两个步骤:
1.预测:
根据运动模型预测下一时刻的系 统状态 (px py vx vy), x_k = F(x)*x_k-1+Q。系统上一时刻状态-系统当前时刻状态, 不涉及状态空间与观测空间的转换, 即不考虑传感器类型。这个过程是线性的, 其状态转换矩阵定义为F(x), 过程噪声定义为Q。
2.测量更新: 根据当前的观测数据, 去修正预测的结果,即计算权重。这里首先需要将预测数据转换到测量空间下, 这个过程涉及到具体的传感器模型, 以及观测带来的传感器本身的噪声R。 z_k = H(x)*x_k + R H矩阵则是描述这个状态空间到观测空间的转换,这样计算残差 delta_x = z_k - x_k。
系统初始化
按卡尔曼滤波的流程,首先需要对系统的状态进行初始化,包括系统状态(px py vx vy),系统协方差矩阵P,传感器测量噪声R。可以通过毫米波,也可以通过激光雷达。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
|
if (!is_initialized_) { ekf_.x_ = VectorXd(4); if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) { float rho = measurement_pack.raw_measurements_[0]; float phi = measurement_pack.raw_measurements_[1]; float rho_dot = measurement_pack.raw_measurements_[2]; float x = rho * cos(phi); float y = rho * sin(phi); float vx = rho_dot * cos(phi); float vy = rho_dot * sin(phi); ekf_.x_ << x, y, vx , vy; } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) { ekf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0; } if (fabs(ekf_.x_(0)) < EPS and fabs(ekf_.x_(1)) < EPS){ ekf_.x_(0) = EPS; ekf_.x_(1) = EPS; }
ekf_.P_ = MatrixXd(4, 4); ekf_.P_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1000, 0, 0, 0, 0, 1000; cout << "EKF init: " << ekf_.x_ << endl; previous_timestamp_ = measurement_pack.timestamp_; is_initialized_ = true; return; }
|
预测
系统初始化完成后,即可进入卡尔曼的递推流程,首先是根据当上一帧系统状态以及运动模型、过程噪声,预测当前的系统状态和不确定度,这个过程不涉及状态空间的转换,即不涉及到具体的传感器模型。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40
|
float dt = (measurement_pack.timestamp_ - previous_timestamp_); dt /= 1000000.0; previous_timestamp_ = measurement_pack.timestamp_; ekf_.F_ = MatrixXd(4, 4); ekf_.F_ << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1; float noise_ax = 9.0; float noise_ay = 9.0; float dt_2 = dt * dt; float dt_3 = dt_2 * dt; float dt_4 = dt_3 * dt; float dt_4_4 = dt_4 / 4; float dt_3_2 = dt_3 / 2; ekf_.Q_ = MatrixXd(4, 4); ekf_.Q_ << dt_4_4 * noise_ax, 0, dt_3_2 * noise_ax, 0, 0, dt_4_4 * noise_ay, 0, dt_3_2 * noise_ay, dt_3_2 * noise_ax, 0, dt_2 * noise_ax, 0, 0, dt_3_2 * noise_ay, 0, dt_2 * noise_ay;
ekf_.Predict();
|
测量更新
测量更新的时候,由于不知道拿到的是激光还是毫米波数据,这里需要分开考虑,如果是激光雷达数据,可以直接作为系统状态,线性转换即可,那么这就是传统的卡尔曼滤波更新即可。如果是毫米波数据,则是非线性变换,需要计算h(x)的雅克比矩阵
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
|
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
ekf_.H_ = tools.CalculateJacobian(ekf_.x_); ekf_.R_ = R_radar_; ekf_.UpdateEKF(measurement_pack.raw_measurements_); } else { ekf_.H_ = H_laser_; ekf_.R_ = R_laser_; ekf_.Update(measurement_pack.raw_measurements_); }
|
根据前面计算的公式来给雅克比矩阵赋值
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
| MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) { float px = x_state(0); float py = x_state(1); float vx = x_state(2); float vy = x_state(3); MatrixXd Hj(3,4); if (fabs(px) < EPS and fabs(py) < EPS){ px = EPS; py = EPS; } float c1 = px*px+py*py; if(fabs(c1) < EPS2){ c1 = EPS2; } float c2 = sqrt(c1); float c3 = (c1*c2); 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; return Hj; }
|
在更新的时候,激光雷达数据的话,将预测转换到观测空间下,计算残差,按卡尔曼流程更新
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
| void KalmanFilter::Update(const VectorXd &z) { VectorXd y = z - H_ * x_; KF(y); }
void KalmanFilter::KF(const VectorXd &y) { MatrixXd Ht = H_.transpose(); MatrixXd S = H_ * P_ * Ht + R_; MatrixXd Si = S.inverse(); MatrixXd K = P_ * Ht * Si;
x_ = x_ + (K * y); int x_size = x_.size(); MatrixXd I = MatrixXd::Identity(x_size, x_size); P_ = (I - K * H_) * P_; }
|
如果是毫米波数据,则是EKF流程先通过之前计算的雅克比,将预测转换到观测空间下,然后按常规卡尔曼滤波更新即可
1 2 3 4 5 6 7 8 9 10 11 12 13
| void KalmanFilter::UpdateEKF(const VectorXd &z) {
double rho = sqrt(x_(0) * x_(0) + x_(1) * x_(1)); double theta = atan(x_(1) / x_(0)); double rho_dot = (x_(0) * x_(2) + x_(1) * x_(3)) / rho; VectorXd h = VectorXd(3); h << rho, theta, rho_dot;
VectorXd y = z - h; KF(y); }
|
最终可以得到融合后的系统状态及不确定度
1 2 3
| cout << "x_ = " << ekf_.x_ << endl; cout << "P_ = " << ekf_.P_ << endl;
|
UKF融合流程
在此流程中,我们将系统状态扩展到5个来叙述,(px py vx vy v)。其流程和EKF中大同小异,不同的是需要初始化UKF的一些特有的参数,比如sigma点的权重、扩散参数等。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40
| use_laser_ = true; use_radar_ = true;
x_ = VectorXd(5); P_ = MatrixXd(5, 5);
std_a_ = 1.5;
std_yawdd_ = 0.57;
std_laspx_ = 0.15;
std_laspy_ = 0.15;
std_radr_ = 0.3;
std_radphi_ = 0.03;
std_radrd_ = 0.3;
n_x_ = x_.size(); n_aug_ = n_x_ + 2; n_sig_ = 2 * n_aug_ + 1;
Xsig_pred_ = MatrixXd(n_x_, n_sig_); lambda_ = 3 - n_aug_; weights_ = VectorXd(n_sig_);
R_radar_ = MatrixXd(3, 3); R_radar_ << std_radr_ * std_radr_, 0, 0, 0, std_radphi_ * std_radphi_, 0, 0, 0, std_radrd_ * std_radrd_;
R_lidar_ = MatrixXd(2, 2); R_lidar_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
|
系统初始化
第一帧数据进来初始化系统当前的状态变量x以及不确定度P。此外还需要计算系统的sigma采样点的权重。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
| if (!is_initialized_) { P_ << 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) { float rho = measurement_pack.raw_measurements_[0]; float phi = measurement_pack.raw_measurements_[1]; float rho_dot = measurement_pack.raw_measurements_[2]; float px = rho * cos(phi); float py = rho * sin(phi); float vx = rho_dot * cos(phi); float vy = rho_dot * sin(phi); float v = sqrt(vx * vx + vy * vy); x_ << px, py, v, 0, 0; } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) { x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0, 0; if (fabs(x_(0)) < EPS and fabs(x_(1)) < EPS) { x_(0) = EPS; x_(1) = EPS; } }
weights_(0) = lambda_ / (lambda_ + n_aug_); for (int i = 1; i < weights_.size(); i++) { weights_(i) = 0.5 / (n_aug_ + lambda_); }
time_us_ = measurement_pack.timestamp_; is_initialized_ = true; return; }
|
预测
这里采用匀加速度模型,并且对纵向加速度和yaw变化率进行估计,所以将系统状态量维度扩展至7维,并计算其采样点集的加权均值和协方差。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
| void UKF::Prediction(double delta_t) { double delta_t2 = delta_t * delta_t;
VectorXd x_aug = VectorXd(n_aug_); MatrixXd P_aug = MatrixXd(n_aug_, n_aug_); MatrixXd Xsig_aug = MatrixXd(n_aug_, n_sig_);
x_aug.fill(0.0); x_aug.head(n_x_) = x_; P_aug.fill(0); P_aug.topLeftCorner(n_x_, n_x_) = P_; P_aug(5, 5) = std_a_ * std_a_; P_aug(6, 6) = std_yawdd_ * std_yawdd_; MatrixXd L = P_aug.llt().matrixL();
Xsig_aug.col(0) = x_aug; double sqrt_lambda_n_aug = sqrt(lambda_ + n_aug_); VectorXd sqrt_lambda_n_aug_L; for (int i = 0; i < n_aug_; i++) { sqrt_lambda_n_aug_L = sqrt_lambda_n_aug * L.col(i); Xsig_aug.col(i + 1) = x_aug + sqrt_lambda_n_aug_L; Xsig_aug.col(i + 1 + n_aug_) = x_aug - sqrt_lambda_n_aug_L; }
for (int i = 0; i < n_sig_; i++) { double p_x = Xsig_aug(0, i); double p_y = Xsig_aug(1, i); double v = Xsig_aug(2, i); double yaw = Xsig_aug(3, i); double yawd = Xsig_aug(4, i); double nu_a = Xsig_aug(5, i); double nu_yawdd = Xsig_aug(6, i); double sin_yaw = sin(yaw); double cos_yaw = cos(yaw); double arg = yaw + yawd * delta_t;
double px_p, py_p; if (fabs(yawd) > EPS) { double v_yawd = v / yawd; px_p = p_x + v_yawd * (sin(arg) - sin_yaw); py_p = p_y + v_yawd * (cos_yaw - cos(arg)); } else { double v_delta_t = v * delta_t; px_p = p_x + v_delta_t * cos_yaw; py_p = p_y + v_delta_t * sin_yaw; } double v_p = v; double yaw_p = arg; double yawd_p = yawd;
px_p += 0.5 * nu_a * delta_t2 * cos_yaw; py_p += 0.5 * nu_a * delta_t2 * sin_yaw; v_p += nu_a * delta_t; yaw_p += 0.5 * nu_yawdd * delta_t2; yawd_p += nu_yawdd * delta_t;
Xsig_pred_(0, i) = px_p; Xsig_pred_(1, i) = py_p; Xsig_pred_(2, i) = v_p; Xsig_pred_(3, i) = yaw_p; Xsig_pred_(4, i) = yawd_p; }
x_ = Xsig_pred_ * weights_; P_.fill(0.0); for (int i = 0; i < n_sig_; i++) { VectorXd x_diff = Xsig_pred_.col(i) - x_; NormAng(&(x_diff(3))); P_ = P_ + weights_(i) * x_diff * x_diff.transpose(); } }
|
测量更新
这里我们按照传感器数据分别去执行测量更新。
1 2 3 4 5 6 7 8 9
| if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR && use_radar_) { UpdateRadar(measurement_pack); } if (measurement_pack.sensor_type_ == MeasurementPackage::LASER && use_laser_) { UpdateLidar(measurement_pack); }
|
对于激光雷达数据作为观测的话,观测维度只有2,而预测的sigma点集的状态维度为7,需要将sigma点集状态转换到测量空间上,即7x15->2x15
1 2 3 4 5 6
| void UKF::UpdateLidar(MeasurementPackage meas_package) { int n_z = 2; MatrixXd Zsig = Xsig_pred_.block(0, 0, n_z, n_sig_); UpdateUKF(meas_package, Zsig, n_z); }
|
对于毫米波数据,其观测维度为2,处理方法类似
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
| void UKF::UpdateRadar(MeasurementPackage meas_package) { int n_z = 3; MatrixXd Zsig = MatrixXd(n_z, n_sig_); for (int i = 0; i < n_sig_; i++) { double p_x = Xsig_pred_(0, i); double p_y = Xsig_pred_(1, i); double v = Xsig_pred_(2, i); double yaw = Xsig_pred_(3, i); double v1 = cos(yaw) * v; double v2 = sin(yaw) * v; Zsig(0, i) = sqrt(p_x * p_x + p_y * p_y); Zsig(1, i) = atan2(p_y, p_x); Zsig(2, i) = (p_x * v1 + p_y * v2) / Zsig(0, i); } UpdateUKF(meas_package, Zsig, n_z); }
|
最终更新UKF,统一到一个函数
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
| void UKF::UpdateUKF(MeasurementPackage meas_package, MatrixXd Zsig, int n_z) { VectorXd z_pred = VectorXd(n_z); z_pred = Zsig * weights_; MatrixXd S = MatrixXd(n_z, n_z); S.fill(0.0);
for (int i = 0; i < n_sig_; i++) { VectorXd z_diff = Zsig.col(i) - z_pred; NormAng(&(z_diff(1))); S = S + weights_(i) * z_diff * z_diff.transpose(); }
MatrixXd R = MatrixXd(n_z, n_z); if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { R = R_radar_; } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { R = R_lidar_; } S = S + R;
MatrixXd Tc = MatrixXd(n_x_, n_z); Tc.fill(0.0); for (int i = 0; i < n_sig_; i++) { VectorXd z_diff = Zsig.col(i) - z_pred; if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { NormAng(&(z_diff(1))); } VectorXd x_diff = Xsig_pred_.col(i) - x_; NormAng(&(x_diff(3))); Tc = Tc + weights_(i) * x_diff * z_diff.transpose(); } VectorXd z = meas_package.raw_measurements_; MatrixXd K = Tc * S.inverse(); VectorXd z_diff = z - z_pred; if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { NormAng(&(z_diff(1))); } x_ = x_ + K * z_diff; P_ = P_ - K * S * K.transpose(); if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { NIS_radar_ = z.transpose() * S.inverse() * z; } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { NIS_laser_ = z.transpose() * S.inverse() * z; } }
|
总结
以上两个小案例非常有助于理解卡尔曼滤波的应用流程,如何去融合当前的传感器数据估计系统状态,较为理论的部分后续再逐渐补充,然后下一篇即完成基于UKF的imu lidar融合定位,也是按照以上流程来实现的,只不过系统状态量扩展到10维,甚至15维。只要把状态方程和观测方程写出来了,原理还是一样。