KF/EKF/UKF融合毫米波和激光跟踪目标(Udacity)

卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波是融合多种传感器数据用于状态估计的王牌算法，在自动驾驶定位领域应用广泛。本章主要利用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。可以通过毫米波，也可以通过激光雷达。

/*****************************************************************************
  * TODO: Initialization
  *  利用第一帧数据, 初始化系统状态变量 ekf_.x_,协方差矩阵P,状态量(px, py, vx, vy)
  *  这里传感器不同, 可以初始化的变量就不同, 比如激光雷达只能初始化px和py
  *  第二帧之后进来的数据则直接进入预测流程
 ****************************************************************************/
 if (!is_initialized_) {
   ekf_.x_ = VectorXd(4);
   if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
     // 利用第一帧毫米波数据初始化状态量
     float rho = measurement_pack.raw_measurements_[0]; // range
  float phi = measurement_pack.raw_measurements_[1]; // bearing
  float rho_dot = measurement_pack.raw_measurements_[2]; // velocity of rho
  // 毫米波数据可同时初始化所有的系统状态变量
  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) {
     // 激光只能初始化px和py
     ekf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
   }
   // 这里校验状态量, 状态量xy太小的话设为EPS
   if (fabs(ekf_.x_(0)) < EPS and fabs(ekf_.x_(1)) < EPS){
	ekf_.x_(0) = EPS;
	ekf_.x_(1) = EPS;
}
// 初始化系统4*4的协方差矩阵P,因为我们不知道速度,所以将vx,vy方差设置大一些
   ekf_.P_ = MatrixXd(4, 4);
   ekf_.P_ << 1, 0, 0, 0,
		   0, 1, 0, 0,
		   0, 0, 1000, 0,
		   0, 0, 0, 1000;
   // Print the initialization results
   cout << "EKF init: " << ekf_.x_ << endl;
   // 更新时间戳，后面需要计算delta_t
   previous_timestamp_ = measurement_pack.timestamp_;
   is_initialized_ = true;
   return;
 }

预测

系统初始化完成后，即可进入卡尔曼的递推流程，首先是根据当上一帧系统状态以及运动模型、过程噪声，预测当前的系统状态和不确定度，这个过程不涉及状态空间的转换，即不涉及到具体的传感器模型。

/*****************************************************************************
  * TODO: Initialization
  *  利用第一帧数据, 初始化系统状态变量 ekf_.x_,协方差矩阵P,状态量(px, py, vx, vy)
  *  这里传感器不同, 可以初始化的变量就不同, 比如激光雷达只能初始化px和py
  *  第二帧之后进来的数据则直接进入预测流程
  /**
  TODO: Prediction
    * 根据上一帧的系统状态(px_1, py_1, vx_1, vy_1)预测当前帧的状态(px py vx vy)
    * 计算状态转换矩阵F->当前预测的状态量x
    * 预测的过程噪声Q, 一部分噪声是由匀速运动模型带来的
  */
 // 计算两帧的时间差dt
 float dt = (measurement_pack.timestamp_ - previous_timestamp_);
 dt /= 1000000.0; // convert micros to s
 previous_timestamp_ = measurement_pack.timestamp_;
 
 // 根据匀速运动模型，设置状态变换的F矩阵
 // px = px_1 + vx*dt
 // py = py_1 + vy*dt
 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;
 // 根据上一时刻状态->预测当前状态,过程噪声协方差矩阵Q
 float dt_2 = dt * dt; //dt^2
 float dt_3 = dt_2 * dt; //dt^3
 float dt_4 = dt_3 * dt; //dt^4
 float dt_4_4 = dt_4 / 4; //dt^4/4
 float dt_3_2 = 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(); // 输入转换矩阵F，过程噪声Q****************************************************************************/

测量更新

测量更新的时候，由于不知道拿到的是激光还是毫米波数据，这里需要分开考虑，如果是激光雷达数据，可以直接作为系统状态，线性转换即可，那么这就是传统的卡尔曼滤波更新即可。如果是毫米波数据，则是非线性变换，需要计算h(x)的雅克比矩阵

 /**
  TODO: Update
    * 根据当前不同类型的观测数据和上一帧预测的状态，来计算残差和卡尔曼增益.
    * 获取当前的状态量以及协方差矩阵.
  */

 if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
   // 当前你观测是毫米波数据的话，H(X)是从状态空间4->测量空间的转换3
// H(X)是非线性的，观测空间->状态空间的变换之后，已经不一定服从高斯分布了
// 所以对H(X)用泰勒一阶展开,因此H矩阵可以由雅克比矩阵J代替
ekf_.H_ = tools.CalculateJacobian(ekf_.x_); // 测量变量3个，状态变量4个，雅克比3*4
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_);
 }

根据前面计算的公式来给雅克比矩阵赋值

MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) {
  // Code from lectures quizes
  float px = x_state(0);
  float py = x_state(1);
  float vx = x_state(2);
  float vy = x_state(3);
  MatrixXd Hj(3,4);
  // Deal with the special case problems
  if (fabs(px) < EPS and fabs(py) < EPS){
	  px = EPS;
	  py = EPS;
  }
  // Pre-compute a set of terms to avoid repeated calculation
  float c1 = px*px+py*py;
  // Check division by zero
  if(fabs(c1) < EPS2){
	c1 = EPS2;
  }
  float c2 = sqrt(c1);
  float c3 = (c1*c2);
  // Compute the Jacobian matrix
  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;
}

在更新的时候，激光雷达数据的话，将预测转换到观测空间下，计算残差，按卡尔曼流程更新

void KalmanFilter::Update(const VectorXd &z) {
  // Kalman filter update step. Equations from the lectures
  VectorXd y = z - H_ * x_; // 残差：当前观测 - H(X)*根据上一帧预测的状态量 = 当前观测 - 预测得到的观测
  KF(y);
}

// Universal update Kalman Filter step. Equations from the lectures
void KalmanFilter::KF(const VectorXd &y) {
  // 观测空间
  // 根据观测的残差计算残差的协方差矩阵、卡尔曼增益
  MatrixXd Ht = H_.transpose(); // H-1
  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_;   // 当前协方差 = （1-K）* 预测的协方差
}

如果是毫米波数据，则是EKF流程先通过之前计算的雅克比，将预测转换到观测空间下,然后按常规卡尔曼滤波更新即可

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(x_)
  h << rho, theta, rho_dot;

  VectorXd y = z - h; // 观测空间： 计算状态量残差 = 观测 - 预测
  // Calculations are essentially the same to the Update function
  KF(y);
}

最终可以得到融合后的系统状态及不确定度

// print the output
cout << "x_ = " << ekf_.x_ << endl;
cout << "P_ = " << ekf_.P_ << endl;

UKF融合流程

在此流程中，我们将系统状态扩展到5个来叙述，(px py vx vy v)。其流程和EKF中大同小异，不同的是需要初始化UKF的一些特有的参数，比如sigma点的权重、扩散参数等。

// 是否融合这些观测数据
use_laser_ = true;
use_radar_ = true;

// 系统的状态量和不确定度
x_ = VectorXd(5);
P_ = MatrixXd(5, 5);

// 纵向加速度上的过程噪声标准差  m/s^2
std_a_ = 1.5;
//  yaw加速度上的过程噪声标准差 in rad/s^2
std_yawdd_ = 0.57;
// 激光雷达在px上测测量噪声标准差 m
std_laspx_ = 0.15;
// 激光雷达在py上测测量噪声标准差 m
std_laspy_ = 0.15;
// 毫米波测量的rho标准差 m
std_radr_ = 0.3;
// 毫米波测量的phi角度标准差
std_radphi_ = 0.03;
// 毫米波测量的角度变化率rho_phi in m/s
std_radrd_ = 0.3;

n_x_ = x_.size();   // 系统状态维度
n_aug_ = n_x_ + 2; // 采样点的状态维度, 这里直接7个维度
n_sig_ = 2 * n_aug_ + 1; // sigma点个数, 15个

Xsig_pred_ = MatrixXd(n_x_, n_sig_);  // 预测的sigma点集状态矩阵 5*15
lambda_ = 3 - n_aug_;  // sigma点扩散参数
weights_ = VectorXd(n_sig_); // sigma点权重

// 初始化测量毫米波和激光的噪声矩阵R
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采样点的权重。

if (!is_initialized_) {
   // 初始化系统协方差P
   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]; // range
     float phi = measurement_pack.raw_measurements_[1]; // bearing
     float rho_dot = measurement_pack.raw_measurements_[2]; // velocity of rho
     // Coordinates convertion from polar to cartesian
     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; // 另外两个数据没有，设为0
   } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
     // 激光雷达数据，只有位置px,py, 其他初始化为0
     x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0, 0;
     // Deal with the special case initialisation problems
     if (fabs(x_(0)) < EPS and fabs(x_(1)) < EPS) {
       x_(0) = EPS;
       x_(1) = EPS;
     }
   }

   // 初始化sigma点权重,一共2*n_aug_+1个sigma点，
   weights_(0) = lambda_ / (lambda_ + n_aug_);  // 第一个点的权重lamda/(n+lamda)
   for (int i = 1; i < weights_.size(); i++) {  // 之后每个点的权重 1/2(n+lamda)
     weights_(i) = 0.5 / (n_aug_ + lambda_);
   }

   time_us_ = measurement_pack.timestamp_;  // 更新时间戳
   is_initialized_ = true;
   //cout << "Init" << endl;
   //cout << "x_" << x_ << endl;
   return;
 }

预测

这里采用匀加速度模型，并且对纵向加速度和yaw变化率进行估计，所以将系统状态量维度扩展至7维，并计算其采样点集的加权均值和协方差。

void UKF::Prediction(double delta_t) {
  // 匀加速运动模型，这里是dt^2
  double delta_t2 = delta_t * delta_t;

  // 扩展到7维的状态
  VectorXd x_aug = VectorXd(n_aug_);    // Augmented mean vector 7×1
  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);    // Augmented state covarience matrix 7×7
  MatrixXd Xsig_aug = MatrixXd(n_aug_, n_sig_);    // Sigma point matrix 7*15

  // Fill the matrices
  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_;  // 剩下两个维度对加速度和yaw变化率噪声进行估计
  P_aug(6, 6) = std_yawdd_ * std_yawdd_;
  MatrixXd L = P_aug.llt().matrixL();  // Square root of P matrix

  // 创建sigma点 7*15
  Xsig_aug.col(0) = x_aug;
  double sqrt_lambda_n_aug = sqrt(lambda_ + n_aug_); // Save some computations
  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;
  }

  // 预测sigma点的状态，根据匀加速运动模型来计算
  for (int i = 0; i < n_sig_; i++) {
    // Extract values for better readability
    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);
    // Precalculate sin and cos for optimization
    double sin_yaw = sin(yaw);
    double cos_yaw = cos(yaw);
    double arg = yaw + yawd * delta_t;

    // Predicted state values
    double px_p, py_p;
    // Avoid division by zero
    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;

    // Add noise
    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;

    // Write predicted sigma point into right column
    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_; // vectorised sum
  // Predicted state covariance matrix
  P_.fill(0.0);
  for (int i = 0; i < n_sig_; i++) {  //iterate over sigma points
    // State difference
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    // Angle normalization
    NormAng(&(x_diff(3)));
    P_ = P_ + weights_(i) * x_diff * x_diff.transpose();
  }
}

测量更新

这里我们按照传感器数据分别去执行测量更新。

// 根据传感器数据类型去获取融合结果
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR && use_radar_) {
  //cout << "Radar " << measurement_pack.raw_measurements_[0] << " " << measurement_pack.raw_measurements_[1] << endl;
  UpdateRadar(measurement_pack);
}
if (measurement_pack.sensor_type_ == MeasurementPackage::LASER && use_laser_) {
  //cout << "Lidar " << measurement_pack.raw_measurements_[0] << " " << measurement_pack.raw_measurements_[1] << endl;
  UpdateLidar(measurement_pack);
}

对于激光雷达数据作为观测的话，观测维度只有2，而预测的sigma点集的状态维度为7，需要将sigma点集状态转换到测量空间上，即7x15->2x15

void UKF::UpdateLidar(MeasurementPackage meas_package) {
  int n_z = 2;  //  设置观测维度
  // 将7*15的sigma 点集合转换到测量空间
  MatrixXd Zsig = Xsig_pred_.block(0, 0, n_z, n_sig_);
  UpdateUKF(meas_package, Zsig, n_z);
}

对于毫米波数据，其观测维度为2，处理方法类似

void UKF::UpdateRadar(MeasurementPackage meas_package) {
  int n_z = 3;   // 观测变量只有3维
  MatrixXd Zsig = MatrixXd(n_z, n_sig_);
  // 将预测的7维状态变量的sigma点转换到3维观测空间
  for (int i = 0; i < n_sig_; i++) {
    // 列为状态空间维度，行为每一个sigma点
    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;
    // Measurement model
    Zsig(0, i) = sqrt(p_x * p_x + p_y * p_y);          //r
    Zsig(1, i) = atan2(p_y, p_x);                   //phi
    Zsig(2, i) = (p_x * v1 + p_y * v2) / Zsig(0, i);   //r_dot
  }
  UpdateUKF(meas_package, Zsig, n_z);
}

最终更新UKF，统一到一个函数

void UKF::UpdateUKF(MeasurementPackage meas_package, MatrixXd Zsig, int n_z) {
  
  VectorXd z_pred = VectorXd(n_z); // 观测状态量 3*1或者2*1 
  z_pred = Zsig * weights_;   // sigma点集加权均值
  MatrixXd S = MatrixXd(n_z, n_z);  //  观测不确定度 3*3或者2*2
  S.fill(0.0);

  // 计算状态量的加权均值
  for (int i = 0; i < n_sig_; i++) {
    // Residual
    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) { // Radar
    R = R_radar_;
  } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { // Lidar
    R = R_lidar_;
  }
  S = S + R;

  // 计算关联矩阵T 5*3或者5*2
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc.fill(0.0);
  for (int i = 0; i < n_sig_; i++) {
    //residual
    VectorXd z_diff = Zsig.col(i) - z_pred;
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
      NormAng(&(z_diff(1)));        // Angle normalization
    }
    // State difference
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    // Angle normalization
    NormAng(&(x_diff(3)));
    Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
  }
  // Measurements
  VectorXd z = meas_package.raw_measurements_;
  //Kalman gain K;
  MatrixXd K = Tc * S.inverse();
  // Residual
  VectorXd z_diff = z - z_pred;
  if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
    // Angle normalization
    NormAng(&(z_diff(1)));
  }
  // Update state mean and covariance matrix
  x_ = x_ + K * z_diff;
  P_ = P_ - K * S * K.transpose();
  // Calculate NIS
  if (meas_package.sensor_type_ == MeasurementPackage::RADAR) { // Radar
    NIS_radar_ = z.transpose() * S.inverse() * z;
  } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) { // Lidar
    NIS_laser_ = z.transpose() * S.inverse() * z;
  }
}

总结

以上两个小案例非常有助于理解卡尔曼滤波的应用流程，如何去融合当前的传感器数据估计系统状态，较为理论的部分后续再逐渐补充，然后下一篇即完成基于UKF的imu lidar融合定位，也是按照以上流程来实现的，只不过系统状态量扩展到10维，甚至15维。只要把状态方程和观测方程写出来了，原理还是一样。