一、無跡卡爾曼濾波器

卡爾曼濾波適用於線性系統，針對於非線性系統很好推廣應用。EKF（擴展卡爾曼濾波）利用線性化的方式，讓狀態和協方差在線性化方程中傳播，但是面對強非線性，這種方式誤差較大，因爲高斯分佈的噪聲經過非線性系統的分佈並不是高斯分佈。UKF利用多個採樣點（無跡變換）在非線性系統中傳播，降低了隨機變量經過非線性系統傳播的誤差，效果強於EKF。
與EKF（擴展卡爾曼濾波）不同,UKF是通過無損變換使非線性系統方程適用於線性假設下的標準Kalman濾波體系,而不是像EKF那樣,通過線性化非線性函數實現遞推濾波。目標跟蹤有兩個理論基礎,即數據關聯和卡爾曼濾波技術. 由於在實際的目標跟蹤中,跟蹤系統的狀態模型和量測模型多是非線性的,因此採用非線性濾波的方法.

二、CTRV模型

在擴展卡爾曼濾波器（EKF）中，我們假設了速度是恆定的，但是這個在現實中顯然是不可能的，現實中物體可能會轉彎，可能會走弧線，所以EKF的預測精度相對就會較差，因爲在EKF中，我們的狀態向量是隻確定了[P_x,P_y,v_x,v_y],沒有考慮到轉向，於是我們就有了新的模型，就是CTRV（恆定轉彎率和速度幅度模型）模型。當然，模型不止一種，還有以下的幾種模型：

恆定轉動率和加速度（CTRA）
恆定轉向角和速度（CSAV）
恆定曲率和加速度（CCA）
恆定轉彎率和速度幅度模型（CTRV）

2.1 狀態向量

對於CTRV模型，我們的狀態向量爲[P_x,P_y,v,ψ,ψ`]^T,每一項的含義如下：

P_x:X軸位置
P_y：Y軸位置
v：速度的大小，是一個標量
ψ：偏航角，轉向角
ψ`：偏航角速度，即轉向角速度

2.2 狀態轉移方程計算

2.2.1 確定部分

我們需要從上一個狀態量來預測下一個狀態，但是顯然這個預測的關係我們沒有辦法直接從圖上得出來，但是不着急，我們一步一步的推。
首先，我們可以根據狀態算出其變化率（change rate of state）：

所以在經過Δt之後，我們認爲新的狀態向量爲：

這個看起來計算量很大，但是因爲狀態向量變化率比較特殊，所以沒那麼難算，經過變換如下圖：

可以使用在線解積分的網站，例如這裏

最終解出來是這樣，這個是在ψ’_k不爲0的時候的解：

當ψ’_k爲0的時候，則
Px’的積分爲：v_kcos(ψ_k)Δt
Py’的積分爲：v_kcos(ψ_k)Δt

2.2.2 噪聲

到此爲止轉移方程中確定的部分已經討論完了，接下來就需要討論不確定的部分，即各種噪聲。

我們用一個二維向量來表示噪聲向量：

這個向量是由兩個相互獨立的噪聲組成，分別如下：

這個主要取決於物體的縱向加速度，所以每一個計算步驟中其實都是改變的。並且服從正態分佈。均值是0，方差是σ_a²。

這個主要取決於物體的角加速度，也是動態的。並且也服從正態分佈。

所以我們把噪聲加入狀態轉移方程;

在這裏我們可以分別求出a，b，c，d，e。

a:
b:
c
d
e

因此有：

對於ψ’_k不爲0的時候：

ψ’_k爲0的時候：

三、無跡卡爾曼濾波器

與EKF（擴展卡爾曼濾波）不同,UKF是通過無損變換使非線性系統方程適用於線性假設下的標準Kalman濾波體系,而不是像EKF那樣,通過線性化非線性函數實現遞推濾波。

3.1 無跡卡爾曼濾波思路

首先看下圖，這是k時刻的狀態向量和狀態協方差：

其中橢圓表示協方差矩陣符合正態分佈，我們要做的就是預測k+1時刻的狀態向量和狀態協方差。

如果是經過一個線性的變換，那麼我們的結果，也就是k+1時刻的狀態向量和協方差矩陣仍然符合正態分佈。
但是如果時經過非線性的變換，那麼就可能會像下圖的黃線區域，不再符合正態分佈。而UKF的目的，就是找到一個正態分佈，儘量接近的去擬合這個非正態分佈，例如圖中的綠色橢圓。

那麼我們的目標就變成了如何尋找這個擬合正態分佈。

3.2 sigma點

想要使用非線性變換轉換整個協方差分佈橢圓非常困難，但是如果我們只轉換幾個點的話式比較容易的。這就是我們要找的sigma點。
sigma點的主要是通過每個狀態向量維度經過一定的公式計算，算出的幾個點。

如上圖，在選好sigma點後，將sigma點帶入非線性變換中，就可以得到變換後的sigma點分佈，然後根據sigma點的分佈擬合出新的一個高斯分佈，那麼這個高斯分佈就基本認爲與我們預測的新的狀態向量和協方差矩陣相似，就可以認爲式預測值。
PS：如果是線性變換，那麼使用sigma點你和出來的其實就是普通的卡爾曼變換。

3.3 無跡卡爾曼濾波器的實現（預測）

3.3.1 計算sigma點

3.3.1.1計算方法

sigma點的數量取決於狀態向量的維度，在這裏，我們用的基於CTRV模型的狀態向量的維度n_x = 5，根據經驗，我們認爲選取n_σ = 2n_x+1個sigma點比較合適，所以這裏選取的n_σ爲11。
其中，第一個點是狀態向量的均值，然後其他的每個維度上都有兩個點，從本維度上擴散

sigma點的計算公式如下圖：

其中需要注意的是，λ是一個設計參數，可以選擇sigma點的擴散方向，一般根據經驗，
人們認爲：
λ = 3 - n_x
是一個比較合適的選擇，同時矩陣的開方本來計算比較困難，但是我們可以直接使用函數計算給出。
其中x_k∣k永遠是第一列，

爲第2列到第n_x +1 列，

爲第n_x +2 列到第2n_x +1 列。

3.3.1.2 關於噪聲向量

在卡爾曼濾波器中，說到噪聲向量，這個表述有時候不是很明確，有的時候是指下圖紅框：

這個噪聲向量跟Δt相關，表示了在兩個步驟之間每一個狀態向量的維度所受的影響。一般在標準卡爾曼濾波器中是指這個。
而有的時候噪聲向量是指這個：

這個向量只是列出了獨立的噪聲，沒有說明這些噪聲對於過程的影響。在無跡卡爾曼濾波器中一般指這個。

我們知道噪聲協方差矩陣Q的計算公式爲：

Q的一個對角線上是0 是因爲縱向加速度噪聲和偏向角加速度噪聲是獨立的。

3.3.1.3 帶噪聲的sigma點計算

因此當我們考慮到噪聲，我們就需要使用增廣矩陣，我們的狀態向量就變成了

所以n_x = 7，那麼n_σ = 2n_a+1 = 15。
這個時候增廣協方差矩陣就是：

3.3.2 sigma點的預測

對於sigma點的預測，這裏比較簡單，只需要將其帶入到我們之前根據模型得出的非線性變換的公式中，但是值得注意的是，我們輸入的狀態向量是帶有噪聲的，但是預測的狀態向量是不帶噪聲的，因此需要做一個維度上的變換。

  VectorXd a = VectorXd(n_x_);
  VectorXd u = VectorXd(n_x_);
  VectorXd xk = VectorXd(n_x_);
  for (int i = 0; i < 2 * n_aug_ + 1; 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 yaw_dot = Xsig_aug(4,i);
    double nu_a = Xsig_aug(5,i);
    double nu_yawdd = Xsig_aug(6,i);
    xk = Xsig_aug.col(i).segment(0, n_x_); // 7 => 5
    u[0] = 0.5 * (delta_t * delta_t) * cos(yaw) * nu_a;
    u[1] = 0.5 * (delta_t * delta_t) * sin(yaw) * nu_a;
    u[2] = delta_t * nu_a;
    u[3] = 0.5 * (delta_t * delta_t) * nu_yawdd;
    u[4] = delta_t * nu_yawdd;

    //avoid division by zero
    if (fabs(yaw_dot) > 0.001)
    {
      a(0) = (v / yaw_dot) * (sin(yaw + yaw_dot * delta_t) - sin(yaw));
      a[1] = (v / yaw_dot) * (cos(yaw) - cos(yaw + yaw_dot * delta_t));
      a[2] = 0;
      a[3] = yaw_dot * delta_t;
      a[4] = 0;
    }
    else
    {
      a[0] = v * cos(yaw) * delta_t;
      a[1] = v * sin(yaw) * delta_t;
      a[2] = 0;
      a[3] = yaw_dot * delta_t;
      a[4] = 0;
    }
    //write predicted sigma points into right column
    Xsig_pred_.col(i) = xk + a + u;
  }

3.3.3 均值和方差的預測

我們使用權重來根據sigma點計算預測的均值和協方差矩陣，具體公式如下，其中i是每一列：

因此，我們就需要計算對於每一列的權重：

我們看到權重的計算主要是跟λ相關，λ是一個設計參數，可以選擇sigma點的擴散方向，所以我們在計算權重的時候也是考慮的這個參數。
因此，X和P的計算公式就是：

  weights_ = VectorXd(2 * n_aug_ + 1);
  weights_(0) = lambda_ / (lambda_ + n_aug_);
  for (int i = 1; i < 2 * n_aug_ + 1; i++)
  {
    weights_(i) = 1 / (2 * (lambda_ + n_aug_));
  }

  //mean
   x_.fill(0.);   // important 
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    x_ += weights_(i) * Xsig_pred_.col(i);
  }
  //Covariance
  P_.fill(0.); //important
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    VectorXd x_diff = Xsig_pred_.col(i) - x_;

    /*
	這裏涉及到了角度，所以需要將角度標準化
    */
    while (x_diff(3) > M_PI)
      x_diff(3) -= 2. * M_PI;
    while (x_diff(3) < -M_PI)
      x_diff(3) += 2. * M_PI;
    P_ += weights_(i) * x_diff * x_diff.transpose();
  }

3.4 無跡卡爾曼濾波器的實現（更新）

3.4.1 預測測量結果

到目前爲止，我們已經成功的使用了k時刻的狀態預測了k+1時刻的狀態，我們需要將狀態向量轉換到測量向量空間（即將狀態向量轉化成與測量向量相同的結構）。
這個過程仍然是一個非線性的變換，所以是以下的一個過程：

因爲是一個非線性的變換，其實我們在這裏還可以使用相同的方式，即選取sigma點，然後變換，然後擬合。但是這裏我們可以偷一下懶，我們可以直接使用現成的我們之前計算過的sigma點，所以可以少計算一步。
所以整體的計算步驟和公式如下圖：

3.4.1.1 雷達數據的處理

對於雷達來說，我們測量到的數據是一個三維的空間，所以我們要將sigma進行一個轉換，如下圖：

需要用到的公式有：

//set measurement dimension, radar can measure r, phi, and r_dot
  int n_z = 3;
  // measurement covariance noise matrix R
  MatrixXd R = MatrixXd(n_z, n_z);
  R << std_radr_ * std_radr_, 0, 0,
      0, std_radphi_ * std_radphi_, 0,
      0, 0, std_radrd_ * std_radrd_;

  //create matrix for sigma points in measurement space
  MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);

  //mean predicted measurement
  VectorXd z_pred = VectorXd(n_z);

  //measurement covariance matrix S
  MatrixXd S = MatrixXd(n_z, n_z);

  //transform sigma points into measurement space
  //calculate Zsig
  Zsig.fill(0.);
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    double px = Xsig_pred_(0,i);
    double py = Xsig_pred_(1,i);
    double v = Xsig_pred_(2,i);
    double yawd = Xsig_pred_(3,i);

    double ro = sqrt(px * px + py * py);
    double theta = atan2(py, px);
    double ro_dot = (px * cos(yawd) * v + py * (sin(yawd) * v)) / ro;


    Zsig(0,i) = ro;
    Zsig(1,i) = theta;
    Zsig(2,i) = ro_dot;
  }

  //calculate mean predicted measurement
  z_pred.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    z_pred += weights_(i) * Zsig.col(i);
  }
  
  std::cout << "z_pred R:" << z_pred << std::endl;
  //calculate measurement covariance matrix S
  S.fill(0.0);
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    VectorXd z_diff = Zsig.col(i) - z_pred;
    //angle normalization
    while (z_diff(1) > M_PI)
      z_diff(1) -= 2. * M_PI;
    while (z_diff(1) < -M_PI)
      z_diff(1) += 2. * M_PI;
    S += weights_(i) * (z_diff) * (z_diff).transpose();
  }
  S = S + R;

3.4.1.2 激光雷達數據的處理

對於激光雷達，我們測量到的數據是一個二維的空間，所以有：

  //set measurement dimension,px,py
  int n_z = 2;
  // measurement covariance noise matrix R
  MatrixXd R = MatrixXd(n_z, n_z);
  R << std_laspx_ * std_laspx_, 0,
      0, std_laspy_ * std_laspy_;

  //create matrix for sigma points in measurement space
  MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);
  Zsig.fill(0.);

  //mean predicted measurement
  VectorXd z_pred = VectorXd(n_z);

  //measurement covariance matrix S
  MatrixXd S = MatrixXd(n_z, n_z);

  //transform sigma points into measurement space
  //calculate Zsig
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    // VectorXd x = Xsig_pred_.col(i);
    // double px = x(0);
    // double py = x(1);

    // Zsig.col(i)(0) = px;
    // Zsig.col(i)(1) = py;
    Zsig(0, i) = Xsig_pred_(0, i);
    Zsig(1, i) = Xsig_pred_(1, i);
  }

  //calculate mean predicted measurement
  z_pred.fill(0.); // important
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    z_pred += weights_(i) * Zsig.col(i);
  }

  //calculate measurement covariance matrix S
  S.fill(0.); // important
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    VectorXd z_diff = Zsig.col(i) - z_pred;
    S += weights_(i) * (z_diff) * (z_diff).transpose();
  }
  S = S + R;

3.4.2 UKF 更新

在這一步，我們終於要引入測量值，就是比我們實際上從傳感器上拉取回來的值，這裏需要計算的是卡爾曼增益K，在計算過程中，我們需要用到一個相關矩陣，用來表示狀態空間中的sigma點和測量空間中的sigma點的關係。

所以卡爾曼增益K就是

狀態向量和協方差矩陣的更新爲：

3.4.2.1 雷達數據更新

 //create matrix for cross correlation Tc
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  // T
  Tc.fill(0.); //important
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    VectorXd t_x_diff = (Xsig_pred_.col(i) - x_);
    while (t_x_diff(3) > M_PI)
      t_x_diff(3) -= 2. * M_PI;
    while (t_x_diff(3) < -M_PI)
      t_x_diff(3) += 2. * M_PI;
    VectorXd z_diff = Zsig.col(i) - z_pred;
    //angle normalization
    while (z_diff(1) > M_PI)
      z_diff(1) -= 2. * M_PI;
    while (z_diff(1) < -M_PI)
      z_diff(1) += 2. * M_PI;
    Tc += weights_(i) * t_x_diff * ((z_diff).transpose());
  }
  // std::cout << "Tc R:" << Tc << std::endl;
  MatrixXd K = Tc * (S.inverse());
  // std::cout << "K R:" << K << std::endl;

  //measurement of radar data
  VectorXd z = VectorXd(n_z);
  z << meas_package.raw_measurements_(0),
      meas_package.raw_measurements_(1),
      meas_package.raw_measurements_(2);
  // std::cout << "Z measurement:" << z << std::endl;

  VectorXd z_diff = z - z_pred;

  while (z_diff(1) > M_PI)
    z_diff(1) -= 2. * M_PI;
  while (z_diff(1) < -M_PI)
    z_diff(1) += 2. * M_PI;

  //update state mean and covariance matrix
  x_ = x_ + K * z_diff;
  P_ = P_ - K * S * K.transpose();

3.4.2.2 激光雷達數據更新

 //create matrix for cross correlation Tc
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc.fill(0);  // important
  // T
  for (int i = 0; i < 2 * n_aug_ + 1; i++)
  {
    VectorXd t_x_diff = (Xsig_pred_.col(i) - x_);
    VectorXd z_diff = Zsig.col(i) - z_pred;
    Tc += weights_(i) * t_x_diff * ((z_diff).transpose());
  }
  MatrixXd K = Tc * (S.inverse());
  //measurement of lidar data
  VectorXd z = VectorXd(n_z);
  z << meas_package.raw_measurements_(0),
      meas_package.raw_measurements_(1);

  VectorXd z_diff = z - z_pred;

  //update state mean and covariance matrix
  x_ = x_ + K * z_diff;
  P_ = P_ - K * S * K.transpose();

四、關於噪聲

對於我們的CTRV模型，我們的噪聲有兩個：

σ_a²,代表了縱向加速度噪聲，也可以理解爲線性加速度噪聲。
σ_ψ’'²，代表了角加速度噪聲。

在項目中，這兩個值往往是來源於硬件的參數，也需要進行一些調整，通常需要一些預估，可以使用被追蹤物體的最大加速度的一半作爲參考值，然後進行微調。

通常要重複這個過程多次：

猜一個差不多的參考值
運行UKF
觀察結果是否滿意
調整參數重試

在這裏我們使用卡方分佈查表來獲取相應的比較合適的噪聲值。

五、完整代碼

完整代碼可以參考我的github

自動駕駛中無跡卡爾曼濾波器的應用（Unscented-Kalman-Filter）

無跡卡爾曼濾波