Haste makes waste

Nano01(自動運転)-U05-Lesson25-Extended Kalman Filters卡尔曼滤波

Posted on By lijun

0. 小结

本章主要讲扩展的卡尔曼滤波器,这种扩展卡尔曼滤波器能够处理不同类型的数据,如雷达和激光雷达,由于雷达数据不同于激光雷达,当接收数据是雷达时,需要使用不同的预测和更新方式,最后描述了如何评价估算结果的方式,所有的方法都有对应C++代码。

  • 参考:
  • EKF Tutorial 浅显易懂的扩展滤波器介绍,而且带有交互式的程序可以确认效果

2. Intro

本模块的实战项目计划,是利用传感器融合追踪一名行人。将激光雷达和雷达的优势劣势结合起来,一起来估算行人位置,方向和速度。这些功能都是在C++中实现。

3. Lesson Map and Fusion Flow

本章中将所有学习到的知识结合起来,开发一个完整的融合模型,基于卡尔曼滤波器的融合包括很多方面。 首先需要创建一个扩展卡尔曼滤波器,扩展的意思是它能处理更复杂的运动模型和测量模型。下面是整体的处理流程:

  • 首先是两个传感器,激光雷达和雷达,它们提供信息用于估算移动中行人的状态。状态通过二维位置和二维速度表示。
  • 每次收到特定传感器的新测量值时,都会触发估算函数,估算函数中,我们执行两个步骤,状态预测测量更新
  • 预测步骤中,我们在协方差中预测行人状态。具体做法是,考虑当前和前一次观察之间的时间差。
  • 测量更新中,将取决于传感器类型,如果当前数据来自激光传感器,我们可以应用一个标准的通用滤波器来更新行人状态。雷达测量值需要使用非线性测量函数。因为我们收到雷达测量值时,会使用不同的方法来处理测量更新,比如,我们可能使用扩展滤波器方程。

image

卡尔曼滤波算法包含如下的步骤:

  1. 首次测量:滤波器将接收自行车相对于汽车位置的初始测量值,这个测量值来自于激光雷达或是雷达。
  2. 初始状态和其协方差矩阵:滤波器基于首次测量值,初始化自行车的位置。
  3. 然后在一定时间间隔Δt后,汽车收到另一个传感器测量值。
  4. 预测:算法将预测在时间Δt后,自行车的位置。一个基础的方式是假设自行车是匀速的。
  5. 更新:滤波器比较预测的位置,以及传感器的测量值,得到一个更新后的位置。这时用到的就是卡尔曼滤波器算法。

4. Lesson Variables and Equations

the derivations of the Kalman Filter equations

5. Estimation Problem Refresh

我们要追踪在无人驾驶车前移动的行人,通用滤波器时一个两步估算问题,预测更新

  1. 开始时,我们依赖已知的行人信息,推断出行人在下次测量到达时的行人状态,这叫做预测步骤
  2. 下一步是更,使用新的观察数据纠正我们对于行人状态的推测值。

image

如果有多个测量值呢,即多个传感器同时进行测量:

image

实际上也是相同的流程,如上图。

image

  • MEAN(X):是一个状态矩阵,包含了需要跟踪物体的位置和速度(velocity)
  • P:是状态的协方差矩阵,包含了物体的位置和速度的不确定性信息
  • k:表示时间跨度
  • k+1 k : 表示预测步骤的值
  • Xk,Pk : 表示经过更新后的值

6. Kalman Filter Intuition

本节是对卡尔曼滤波的详解,可以参考卡尔曼滤波器最佳线性滤波器原理

下面是预测更新流程:

image

卡尔曼滤波的预测与更新公式:

image

预测:

上面左侧的预测公式:

  1. F表示状态迁移矩阵,v表示加速度减速度等不确定性因为在这段时间内产生的不确定性。x表示前一个时间段的位置和速度状态,x’表示预测的下一个时间段状态。
  2. P表示状态的协方差矩阵,即这个预测所产生的不确定性,FPF(T)表示迁移过来的协方差,后面的Q表示预测模型本身带来的噪声。

更新:

  1. y=z−Hx',y表示观测本身带来的噪声,z表示观测值
  2. K矩阵,通常也叫做卡尔曼滤波增量,它结合了预测带来的噪声P’和测量传感器的噪声R,这个矩阵可以根据噪声P和R的大小,调整到底相信预测多一点,还是相信测量值多一点。

7. Kalman Filter Equations in C++ Part 1

后面的练习中,使用到一个库 Eigen Library,参考资料here

常见使用方法的示例代码如下:

#include "Eigen/Dense"

VectorXd my_vector(2);
my_vector << 10, 20;
cout << my_vector << endl;

MatrixXd my_matrix(2,2);
my_matrix << 1, 2,
             3, 4;

my_matrix(1,0) = 11;    //second row, first column
my_matrix(1,1) = 12;    //second row, second column

MatrixXd my_matrix_t = my_matrix.transpose();
MatrixXd my_matrix_i = my_matrix.inverse();

MatrixXd another_matrix;
another_matrix = my_matrix*my_vector;
/** 
 * Write a function 'filter()' that implements a multi-
 *   dimensional Kalman Filter for the example given
 */

#include <iostream>
#include <vector>
#include "Dense"

using std::cout;
using std::endl;
using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kalman Filter variables
VectorXd x;	// object state
MatrixXd P;	// object covariance matrix
VectorXd u;	// external motion
MatrixXd F; // state transition matrix
MatrixXd H;	// measurement matrix
MatrixXd R;	// measurement covariance matrix
MatrixXd I; // Identity matrix
MatrixXd Q;	// process covariance matrix

vector<VectorXd> measurements;
void filter(VectorXd &x, MatrixXd &P);


int main() {
  /**
   * Code used as example to work with Eigen matrices
   */
  // design the KF with 1D motion
  x = VectorXd(2);
  x << 0, 0;

  P = MatrixXd(2, 2);
  P << 1000, 0, 0, 1000;

  u = VectorXd(2);
  u << 0, 0;

  F = MatrixXd(2, 2);
  F << 1, 1, 0, 1;

  H = MatrixXd(1, 2);
  H << 1, 0;

  R = MatrixXd(1, 1);
  R << 1;

  I = MatrixXd::Identity(2, 2);

  Q = MatrixXd(2, 2);
  Q << 0, 0, 0, 0;

  // create a list of measurements
  VectorXd single_meas(1);
  single_meas << 1;
  measurements.push_back(single_meas);
  single_meas << 2;
  measurements.push_back(single_meas);
  single_meas << 3;
  measurements.push_back(single_meas);

  // call Kalman filter algorithm
  filter(x, P);

  return 0;
}


void filter(VectorXd &x, MatrixXd &P) {

  for (unsigned int n = 0; n < measurements.size(); ++n) {

    VectorXd z = measurements[n];
    // TODO: YOUR CODE HERE
    /**
     * KF Measurement update step
     */
    VectorXd y = z - H * x; // 新测量值z时的误差计算
    MatrixXd Ht = H.transpose(); //h矩阵转置
    MatrixXd S = H * P * Ht + R; //s矩阵
    MatrixXd Si = S.inverse(); //s矩阵的求逆矩阵
    MatrixXd K =  P * Ht * Si; //卡尔曼增益

    // new state
    x = x + (K * y); //预测后的状态
    P = (I - K * H) * P; //预测后的协方差

    /**
     * KF Prediction step
     */
    x = F * x + u;
    MatrixXd Ft = F.transpose();
    P = F * P * Ft + Q;

    cout << "x=" << endl <<  x << endl;
    cout << "P=" << endl <<  P << endl;
  }
}

输出:

x=
0.999001
       0
P=
1001 1000
1000 1000
x=
   2.998
0.999002
P=
4.99002 2.99302
2.99302 1.99501
x=
3.99967
      1
P=
 2.33189 0.999168
0.999168 0.499501

参考代码(python)here

def kalman_filter(x, P):
    print(x)
    for n in range(len(measurements)):
        # measurement update
        Z = matrix([[measurements[n]]])
        y = Z - (H * x)
        S = H * P * H.transpose()  + R
        K = P * H.transpose() * S.inverse()
        P = (I - (K*H)) * P
        
        x = x + (K * y)                     
                              
        # prediction
        P = F * P * F.transpose()
        x = (F*x) + u              
    return x,P

9. State Prediction

上面用C++实现了一个卡尔曼滤波器来预测更新一维数据,现在驾驶传感器提供的是二维测量数据。

image

还有一点与之前不同,就是时间差不是固定的了:

image

x'=Fx+noise 中noise是指在预测位置时产生得不确定性,模型假设在一定间隔内是匀速的,但是实际上物体可能加速减速,那这个模型就通过noise来表示这种不确定性。

测量误差也是存在的,表示传感器测量带来的不确定性。

Quiz1:比较时间间隔0.1秒,和时间间隔5秒,哪个产生的不确定性高,显然是5秒产生的不确定性更大,可以简单的将5秒分成50个0.1秒,而且这连续的50个0.1s中没有反馈可以纠正数据,那么5秒的不确定性,也就是噪声显然是大于0.1秒。 Quiz2:一个行人在一段时间内,有加速有减速,有停止,那这个会给预测模型带来很大的噪声。

From the examples I’ve just showed you we can clearly see that the process noise depends on both:

  1. the elapsed time
  2. the uncertainty of acceleration.

10. Process Covariance Matrix(协方差)

最终状态更新的时候,我们需要将处理(预测时)协方差矩阵转换成状态协方差矩阵

下面是处理协方差矩阵的计算过程:

image

image

更加详细推导过程参考下图:

image image image

11. Laser Measurements Part 1

这里需要将预测得到的状态向量,转换为测量得到的位置向量,状态向量中有4维度分别是x和y方向上的位置,以及速度,位置向量就是x和y方向的位置。

image

H是一个观测向量,用于将二维的x状态向量,转换成与z一样的标量(1*p + 0*v得到位置p,z也是位置信息)。

13. Laser Measurements Part 3

image

R表示测量的协方差矩阵,用于衡量接收到 激光传感器位置值的不确定性。

下面的编程练习中,实现一个卡尔曼滤波器,该滤波器可以代入激光雷达测量值,并在二维平面上追踪行人,如下图中,行人的状态由右边的state vector表示:

image

代码主要分为三个类:kalman_filtertrackingmeasurement_package。在这个练习中,需要根据当前和之前的测量值之间的时间差,修改F和Q的矩阵。

image

最后添加的代码在tracking类中的ProcessMeasurement函数中。

13.1 main.cpp

#include <iostream>
#include <sstream>
#include <vector>
#include "Dense"
#include "measurement_package.h"
#include "tracking.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::ifstream;
using std::istringstream;
using std::string;
using std::vector;


int main() {

  /**
   * Set Measurements
   */
  vector<MeasurementPackage> measurement_pack_list;

  // hardcoded input file with laser and radar measurements
  string in_file_name_ = "obj_pose-laser-radar-synthetic-input.txt";
  ifstream in_file(in_file_name_.c_str(), ifstream::in);

  if (!in_file.is_open()) {
    cout << "Cannot open input file: " << in_file_name_ << endl;
  }

  string line;
  // set i to get only first 3 measurments
  int i = 0;
  while (getline(in_file, line) && (i<=3)) {

    MeasurementPackage meas_package;

    istringstream iss(line);
    string sensor_type;
    iss >> sensor_type; // reads first element from the current line
    int64_t timestamp;
    if (sensor_type.compare("L") == 0) {  // laser measurement
      // read measurements
      meas_package.sensor_type_ = MeasurementPackage::LASER;
      meas_package.raw_measurements_ = VectorXd(2);
      float x;
      float y;
      iss >> x;
      iss >> y;
      meas_package.raw_measurements_ << x,y;
      iss >> timestamp;
      meas_package.timestamp_ = timestamp;
      measurement_pack_list.push_back(meas_package);

    } else if (sensor_type.compare("R") == 0) {
      // Skip Radar measurements
      continue;
    }
    ++i;
  }

  // Create a Tracking instance
  Tracking tracking;

  // call the ProcessingMeasurement() function for each measurement
  size_t N = measurement_pack_list.size();
  // start filtering from the second frame 
  // (the speed is unknown in the first frame)
  for (size_t k = 0; k < N; ++k) {
    tracking.ProcessMeasurement(measurement_pack_list[k]);
  }

  if (in_file.is_open()) {
    in_file.close();
  }
  return 0;
}

13.2 kalman_filter.cpp

#include "kalman_filter.h"

KalmanFilter::KalmanFilter() {
}

KalmanFilter::~KalmanFilter() {
}

void KalmanFilter::Predict() {
  x_ = F_ * x_;
  MatrixXd Ft = F_.transpose();
  P_ = F_ * P_ * Ft + Q_;
}

void KalmanFilter::Update(const VectorXd &z) {
  VectorXd z_pred = H_ * x_;
  VectorXd y = z - z_pred;
  MatrixXd Ht = H_.transpose();
  MatrixXd S = H_ * P_ * Ht + R_;
  MatrixXd Si = S.inverse();
  MatrixXd PHt = P_ * Ht;
  MatrixXd K = PHt * Si;

  //new estimate
  x_ = x_ + (K * y);
  long x_size = x_.size();
  MatrixXd I = MatrixXd::Identity(x_size, x_size);
  P_ = (I - K * H_) * P_;
}

13.3 kalman_filter.h

#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_

#include "Dense"

using Eigen::MatrixXd;
using Eigen::VectorXd;

class KalmanFilter {
 public:

  /**
   * Constructor
   */
  KalmanFilter();

  /**
   * Destructor
   */
  virtual ~KalmanFilter();

  /**
   * Predict Predicts the state and the state covariance
   *   using the process model
   */
  void Predict();

  /**
   * Updates the state and
   * @param z The measurement at k+1
   */
  void Update(const VectorXd &z);
  
  // state vector
  VectorXd x_;

  // state covariance matrix
  MatrixXd P_;

  // state transistion matrix
  MatrixXd F_;

  // process covariance matrix
  MatrixXd Q_;

  // measurement matrix
  MatrixXd H_;

  // measurement covariance matrix
  MatrixXd R_;

};

#endif  // KALMAN_FILTER_H_

13.4 tracking.cpp - 修改后的代码

#include "tracking.h"
#include <iostream>
#include "Dense"
#include <cmath>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::vector;

Tracking::Tracking() {
  is_initialized_ = false;
  previous_timestamp_ = 0;

  // create a 4D state vector, we don't know yet the values of the x state
  kf_.x_ = VectorXd(4);

  // state covariance matrix P
  kf_.P_ = MatrixXd(4, 4);
  kf_.P_ << 1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1000, 0,
            0, 0, 0, 1000;


  // measurement covariance
  kf_.R_ = MatrixXd(2, 2);
  kf_.R_ << 0.0225, 0,
            0, 0.0225;

  // measurement matrix
  kf_.H_ = MatrixXd(2, 4);
  kf_.H_ << 1, 0, 0, 0,
            0, 1, 0, 0;

  // the initial transition matrix F_
  kf_.F_ = MatrixXd(4, 4);
  kf_.F_ << 1, 0, 1, 0,
            0, 1, 0, 1,
            0, 0, 1, 0,
            0, 0, 0, 1;

  // set the acceleration noise components
  noise_ax = 5;
  noise_ay = 5;
}

Tracking::~Tracking() {

}

// Process a single measurement
void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
  if (!is_initialized_) {
    //cout << "Kalman Filter Initialization " << endl;

    // set the state with the initial location and zero velocity
    kf_.x_ << measurement_pack.raw_measurements_[0], 
              measurement_pack.raw_measurements_[1], 
              0, 
              0;

    previous_timestamp_ = measurement_pack.timestamp_;
    is_initialized_ = true;
    return;
  }

  // compute the time elapsed between the current and previous measurements
  // dt - expressed in seconds
  float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
  previous_timestamp_ = measurement_pack.timestamp_;
  
  // TODO: YOUR CODE HERE
  // 1. Modify the F matrix so that the time is integrated
  // 2. Set the process covariance matrix Q
  // 3. Call the Kalman Filter predict() function
  // 4. Call the Kalman Filter update() function
  //      with the most recent raw measurements_
  
   // TODO: YOUR CODE HERE
  float dt_2 = dt * dt;
  float dt_3 = dt_2 * dt;
  float dt_4 = dt_3 * dt;

  // Modify the F matrix so that the time is integrated
  kf_.F_(0, 2) = dt;
  kf_.F_(1, 3) = dt;

  // set the process covariance matrix Q
  kf_.Q_ = MatrixXd(4, 4);
  kf_.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;

  // predict
  kf_.Predict();

  // measurement update
  kf_.Update(measurement_pack.raw_measurements_);
  cout << "x_= " << kf_.x_ << endl;
  cout << "P_= " << kf_.P_ << endl;
}

13.5 tracking.h

#ifndef TRACKING_H_
#define TRACKING_H_

#include <vector>
#include <string>
#include <fstream>
#include "kalman_filter.h"
#include "measurement_package.h"

class Tracking {
 public:
  Tracking();
  virtual ~Tracking();
  void ProcessMeasurement(const MeasurementPackage &measurement_pack);
  KalmanFilter kf_;

 private:
  bool is_initialized_;
  int64_t previous_timestamp_;

  //acceleration noise components
  float noise_ax;
  float noise_ay;

};

#endif  // TRACKING_H_

13.6 measurement_package.h

#ifndef MEASUREMENT_PACKAGE_H_
#define MEASUREMENT_PACKAGE_H_

#include "Dense"

class MeasurementPackage {
 public:

  enum SensorType {
    LASER, RADAR
  } sensor_type_;

  Eigen::VectorXd raw_measurements_;
  
  int64_t timestamp_;

};

#endif  // MEASUREMENT_PACKAGE_H_

最后输出:

x_=  0.96749
0.405862
 4.58427
-1.83232
P_= 0.0224541         0  0.204131         0
        0 0.0224541         0  0.204131
 0.204131         0   92.7797         0
        0  0.204131         0   92.7797
x_= 0.958365
0.627631
0.110368
 2.04304
P_= 0.0220006         0  0.210519         0
        0 0.0220006         0  0.210519
 0.210519         0   4.08801         0
        0  0.210519         0   4.08801
x_=   1.34291
 0.364408
  2.32002
-0.722813
P_= 0.0185328         0  0.109639         0
        0 0.0185328         0  0.109639
 0.109639         0   1.10798         0
        0  0.109639         0   1.10798

14. Laser Measurements Part 4

使用公式:

image

更新的代码如下:

// Process a single measurement
void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
  if (!is_initialized_) {
    //cout << "Kalman Filter Initialization " << endl;

    // set the state with the initial location and zero velocity
    kf_.x_ << measurement_pack.raw_measurements_[0], 
              measurement_pack.raw_measurements_[1], 
              0, 
              0;

    previous_timestamp_ = measurement_pack.timestamp_;
    is_initialized_ = true;
    return;
  }

  // compute the time elapsed between the current and previous measurements
  // dt - expressed in seconds
  float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
  previous_timestamp_ = measurement_pack.timestamp_;
  
    // TODO: YOUR CODE HERE
  float dt_2 = dt * dt;
  float dt_3 = dt_2 * dt;
  float dt_4 = dt_3 * dt;

  // Modify the F matrix so that the time is integrated
  kf_.F_(0, 2) = dt;
  kf_.F_(1, 3) = dt;

  // set the process covariance matrix Q
  kf_.Q_ = MatrixXd(4, 4);
  kf_.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;

  // predict
  kf_.Predict();

  // measurement update
  kf_.Update(measurement_pack.raw_measurements_);
  
  cout << "x_= " << kf_.x_ << endl;
  cout << "P_= " << kf_.P_ << endl;
}

15. Radar Measurements

image

上面的14节的例子中,只有一个激光雷达,激光雷达只能探测到位置信息,雷达虽然精度不高,但是能获取速度信息,将这两者融合能得到更高的精度。

下面是雷达获取的测量值,与激光雷达不同,包含了:

  1. 径向距离
  2. 方向角
  3. 径向速度

image

利用卡尔曼滤波器公式:

image

h(x’)将预测向量,转换成测量向量的形式:

image

这个函数是个非线性函数,将预测值的位置和速度(笛卡尔坐标),转换成极坐标系(径向半径,径向速度,方向角)的形式。

详细的推导过程如下:

image image image

16. Mapping with a Nonlinear Function

What happens if we have a nonlinear measurement function, h(x). Can we apply the Kalman Filter equations to update the predicted state, X, with new measurements, z?

问题:能使用卡尔曼滤波器,来更新测量值z和预测值X吗?

Answer:No, We aren’t working with Gaussian distributions after applying a nonlinear measurement function. 还需要对非线性处理结果进行高斯分布处理。

17. Extended Kalman Filter

如上所述,应用h(x’)函数后,就不是高斯分布了,那么卡尔曼滤波器也不再适用了。

如下图,高斯分布的值,在经过h(x’)函数后:

image

所以需要对h(x)函数进行线性化操作,这个是扩展卡尔曼滤波器的核心思想

扩展卡尔曼滤波器使用了一阶泰勒展开法,将非线性函数进行线性化处理。

关于泰勒展开式,参考here

18. Multivariate Taylor Series Expansion

image

上面的函数,在mu=0处的一阶泰勒展开式 h(x)≈x

image

19. Jacobian Matrix Part 1

image

Hj矩阵形式为:

image

用代码实现为:

#include <iostream>
#include <vector>
#include "Dense"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;

MatrixXd CalculateJacobian(const VectorXd& x_state);

int main() {
  /**
   * Compute the Jacobian Matrix
   */

  // predicted state example
  // px = 1, py = 2, vx = 0.2, vy = 0.4
  VectorXd x_predicted(4);
  x_predicted << 1, 2, 0.2, 0.4;

  MatrixXd Hj = CalculateJacobian(x_predicted);

  cout << "Hj:" << endl << Hj << endl;

  return 0;
}

MatrixXd CalculateJacobian(const VectorXd& x_state) {

  MatrixXd Hj(3,4);
  // recover state parameters
  float px = x_state(0);
  float py = x_state(1);
  float vx = x_state(2);
  float vy = x_state(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) {
    cout << "CalculateJacobian () - Error - Division by Zero" << endl;
    return Hj;
  }

  // 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;
}

21. EKF Algorithm Generalization

对比卡尔曼滤波与扩展卡尔曼滤波的区别:

image

  1. 计算预测协方差的时候,使用Fj代替F矩阵
  2. 计算S/K/P时,使用Hj矩阵代替H矩阵
  3. 计算x’时,使用预测更新函数f,代替F矩阵
  4. 计算y时,使用h函数代替H矩阵

image

Quiz:

Compared to Kalman Filters, how would the Extended Kalman Filter result differ when the prediction function and measurement function are both linear? -> The Extended Kalman Filter’s result would be the same as the standard kalman Filter’s result.

If f and h are linear functions, then the Extended Kalman Filter generates exactly the same result as the standard Kalman Filter. Actually, if f and h are linear then the Extended Kalman Filter F_j turns into f and H_j turns into h. All that’s left is the same ol’ standard Kalman Filter!

In our case we have a linear motion model, but a nonlinear measurement model when we use radar observations. So, we have to compute the Jacobian only for the measurement function.

22. Sensor Fusion General Flow

有一名正在移动的行人,他的状态通过二维位置和二维速度表示,每次收到传感器过来的值,都会触发估算函数:Process Measurement

  1. 第一次迭代,只是初始化状态和协方差矩阵。
  2. 然后,我们调用了预测和测量更新。
  3. 在预测前,先计算前后两次的时间差。
  4. 然后用时间差,计算新的状态转换和过程协方差矩阵。
  5. 测量更新,取决于传感器类型,如果当前是雷达类型,就必须要计算新的额雅克比矩阵Hj,使用非线性函数来预估预测状态,并调用测量更新。
  6. 如果当前是激光传感器,我们只需要使用激光的H和R矩阵设置扩展卡尔曼滤波器,然后调用测量更新。

23. Evaluating KF Performance Part 1

image

用上述公式评价估算结果与真实结果的差别,C++代码实现如下:

#include <iostream>
#include <vector>
#include "Dense"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::vector;

VectorXd CalculateRMSE(const vector<VectorXd> &estimations,
    const vector<VectorXd> &ground_truth);

int main() {
  /**
   * Compute RMSE
   */
  vector<VectorXd> estimations;
  vector<VectorXd> ground_truth;

  // the input list of estimations
  VectorXd e(4);
  e << 1, 1, 0.2, 0.1;
  estimations.push_back(e);
  e << 2, 2, 0.3, 0.2;
  estimations.push_back(e);
  e << 3, 3, 0.4, 0.3;
  estimations.push_back(e);

  // the corresponding list of ground truth values
  VectorXd g(4);
  g << 1.1, 1.1, 0.3, 0.2;
  ground_truth.push_back(g);
  g << 2.1, 2.1, 0.4, 0.3;
  ground_truth.push_back(g);
  g << 3.1, 3.1, 0.5, 0.4;
  ground_truth.push_back(g);

  // call the CalculateRMSE and print out the result
  cout << CalculateRMSE(estimations, ground_truth) << endl;


  return 0;
}

VectorXd CalculateRMSE(const vector<VectorXd> &estimations,
    const vector<VectorXd> &ground_truth) {

  VectorXd rmse(4);
  rmse << 0,0,0,0;

  // check the validity of the following inputs:
  //  * the estimation vector size should not be zero
  //  * the estimation vector size should equal ground truth vector size
  if (estimations.size() != ground_truth.size()
      || estimations.size() == 0) {
    cout << "Invalid estimation or ground_truth data" << endl;
    return rmse;
  }

  // accumulate squared residuals
  for (unsigned int i=0; i < estimations.size(); ++i) {

    VectorXd residual = estimations[i] - ground_truth[i];

    // coefficient-wise multiplication
    residual = residual.array()*residual.array();
    rmse += residual;
  }

  // calculate the mean
  rmse = rmse/estimations.size();

  // calculate the squared root
  rmse = rmse.array().sqrt();

  // return the result
  return rmse;
}

输出:

0.1
0.1
0.1
0.1

25. Outro

通过上面的代码,从大的层面上,已经设计了融合系统,该系统能够融合激光和雷达传感器提供的测量值,这样我们就能估算行人的位置和速度。

后面还学习了如果实现卡尔曼滤波器,以及非线性测量模型的操作,后面还要深入学习如何处理非线性运动,如汽车的转弯等,你需要创建无损卡尔曼滤波器

26. Bonus Round: Sensor Fusion [Optional]