無人駕駛技術——激光雷達測量

激光雷達測量模型

參數定義

測量向量z

z是測量矢量。對於激光雷達傳感器，矢量z是包含位置x和位置y測量。

測量矩陣H

H是將你對物體當前狀態的信息投射到傳感器測量空間的矩陣。對於激光雷達來說，這是一種奇特的說法，即我們丟棄狀態變量中的速度信息，因爲激光雷達傳感器只測量位置：狀態向量x是一個四維向量， 包含關於位置信息

矢量z將只包含[px，py]，乘以hx，我們可以比較x，我們的信仰，和z，傳感器測量值。

x向量中的素數符號代表什麼意思呢？
Px’意味着您已經完成了預測步驟，但尚未完成測量步驟。換言之，這個物體是在位置Px, 經過一段時間 t_t 之後 。你計算出你認爲物體將基於運動模型的位置，得到Px’

舉個例子，如何找到正確的H矩陣，從4d狀態投影到2d觀察空間，如下所示：

爲了獲得我們的測量值（x和y），我們需要確定H矩陣。（x和y）是用H乘以狀態向量得到的。

我們先來找出H矩陣的維數。這裏我們有一個2乘1的矩陣，它來自m乘n乘h的矩陣乘以四行一列的矩陣。現在，從矩陣乘法中我們知道，第一個矩陣的列數應該等於第二個矩陣的行數，即4。並且第一個矩陣的行數與結果矩陣的行數相同，即2。所以我們的h是一個2行4列的矩陣。最後，我們把1和0放進去，使px和py座標傳播到結果z。

協方差矩陣R

測量噪聲協方差矩陣R

現在，讓我們來看一下協方差矩陣，R，它表示傳感器測量中的不確定性。R矩陣的尺寸爲正方形，其每邊的長度與測量參數的數量相同。

對於激光傳感器，我們有一個二維測量矢量。每個位置分量px，py都受到隨機噪聲的影響。所以我們的噪聲向量ω與z的維數相同。它是一個平均值爲零的分佈，協方差矩陣是2×2，來自於垂直向量ω和它的轉置的乘積。

其中R是測量噪聲協方差矩陣；換句話說，矩陣R代表我們從激光傳感器接收到的位置測量的不確定性。

通常，隨機噪聲測量矩陣的參數將由傳感器製造商提供。對於擴展卡爾曼濾波項目，我們爲雷達傳感器和激光雷達傳感器提供了R矩陣值。

記住，R中的非對角0表示噪聲過程不相關。

編程實現激光跟蹤

下面我們就來實現一個在二維座標系中，利用Kalman濾波器進行追蹤行人。

激光雷達測量能預測到行人的位置x和y，但是沒有速度信息。
其中F矩陣和Q矩陣如下：

tracking.cpp

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

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
  
  float dt_2 = dt * dt;
  float dt_3 = dt_2 * dt;
  float dt_4 = dt_3 * dt;
  
  // 1. Modify the F matrix so that the time is integrated
  
  kf_.F_(0,2) = dt;
  kf_.F_(1,3) = dt;
  
  // 2. Set the process covariance matrix Q
  kf_.Q_ = MatrixXd(4, 4);
  kf_.Q_ << dt_4 *noise_ax/4 , 0, dt_3 *noise_ax/2, 0,
            0, dt_4 *noise_ay/4, 0, dt_3 *noise_ay/2,
            dt_3 *noise_ax/2, 0, dt_2*noise_ax, 0,
            0, dt_3 *noise_ay/2, 0, dt_2*noise_ay;
  
  // 3. Call the Kalman Filter predict() function
  kf_.Predict();
  // 4. Call the Kalman Filter update() function
  //      with the most recent raw measurements_
  kf_.Update(measurement_pack.raw_measurements_);
  
  cout << "x_= " << kf_.x_ << endl;
  cout << "P_= " << kf_.P_ << endl;
}

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_

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_

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

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_

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

運行結果如下：

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