無跡卡爾曼濾波器詳解

@

目錄

• 一、 非線性處理/測量模型
• 二、無損（跡）變換（Unscented Transformation）
  • 2.1 一個高斯分布產生sigma point
  • 2.2 sigma point的權重
  • 2.3 預測新的狀態分布（predict過程）
  • 2.4 更新濾波器（measurement過程）

一、 非線性處理/測量模型

我們知道KF是面臨的主要問題就是非線性處理模型（比如說CTRV）和非線性測量模型（RADAR測量）的處理。我們從概率分布的角度來描述這個問題：

對於我們想要估計的狀態，在\(k\)時刻滿足均值為\(x_k\),方差為\(P_k\) 這樣的一個高斯分布，這個是我們在k時刻的 后驗(Posterior)（如果我們把整個卡爾曼濾波的過程迭代的來考慮的話），現在我們以這個后驗為出發點，結合一定的先驗知識（比如說CTRV運動模型）去估計我們在 k+1時刻的狀態的均值和方差，這個過程就是卡爾曼濾波的預測，如果變換是線性的，那么預測的結果仍然是高斯分布，但是現實是我們的處理和測量模型都是非線性的，結果就是一個不規則分布，KF能夠使用的前提就是所處理的狀態是滿足高斯分布的，為了解決這個問題，EKF是尋找一個線性函數來近似這個非線性函數，而UKF就是去找一個與真實分布近似的高斯分布。

UKF的基本思路就是： 近似非線性函數的概率分布要比近似非線性函數本身要容易！

那么如何去找一個與真實分布近似的高斯分布呢？——找一個與真實分布有着相同的均值和協方差的高斯分布。那么如何去找這樣的均值和方差呢？——無損變換。

UT變換是用固定數量的參數去近似一個高斯分布，其實現原理為：在原先分布中按某一規則取一些點，使這些點的均值和協方差與原狀態分布的均值和協方差相等；將這些點代入非線性函數中，相應得到非線性函數值點集，通過這些點集可求取變換的均值和協方差。對任何一種非線性系統，當高斯型狀態微量經由非線性系統進行傳遞，進而利用這組采樣點能獲取精確到三階矩的后驗均值和協方差。

UT變換的特點是對非線性函數的概率密度分布進行近似，而不是對非線性函數進行近似，即使系統模型復雜，也不增加算法實現的難度；而且所得到的非線性函數的統計量的准確性可以達到三階；除此之外，它不需要計算雅可比矩陣，可以處理不可導非線性函數。

二、無損（跡）變換（Unscented Transformation）

通過一定的手段產生的這些sigma點能夠代表當前的分布，然后將這些點通過非線性函數（系統模型）變換成一些新的點，然后基於這些新的sigma點計算出一個高斯分布（帶有權重地計算出來）

2.1 一個高斯分布產生sigma point

UT變換基本原理如下：假設一個非線性系統\(y=g(x)\)，其中\(x\)為\(n\)維狀態向量，並已知其平均值為\(\overline x\)，方差為\(P_x\)，則可以經過UT變換構造\(2n+1\)個Sigma點，同時構造相應的權值，進而得到y的統計特性。

構造sigma點集的計算公式為：

其中的 λ 是比例因子，根據公式，λ 越大， sigma點就越遠離狀態的均值，λ 越小， sigma點就越靠近狀態的均值。

\[\lambda = \alpha(n^2+k)-n \]

\((\sqrt {(n+\lambda)P_x})_i\)表示矩陣方根\(\sqrt {(n+\lambda)P_x}\)的第\(i\)列。

可以使用Cholesky分解計算出矩陣\(L(Σ = LL^T)\)

  //calculate square root of P
  MatrixXd A=P.llt().matrix();

/**
   * @brief compute sigma points  生成sigma points
   * @param mean          mean
   * @param cov           covariance
   * @param sigma_points  calculated sigma points
   */
  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size(); // 狀態x的維度
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt;
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();

    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i);
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
    }
  }

\(\overline x\)周圍Sigma點的分布狀態由\(\alpha\)決定，調節\(\alpha\)可以降低高階項的影響，通常設為一個較小的正數，這里選取\(\alpha=0.001\)。\(k\)的取值沒有具體設定限制，但至少應當保證矩陣\({(n+\lambda)P_x}\)為半正定矩陣，通常設置\(k=0\)。

2.2 sigma point的權重

權重的選擇應該滿足下面的性質，假設y=g(x)

因此權重w的公式如下：

如果x是高斯分布,\(\beta=2\)是最佳的

	// 均值的權重
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

2.3 預測新的狀態分布（predict過程）

1. 使用系統方程更新所有sigma points的狀態

\[\hat \sigma=f(\sigma) \]

// 遍歷每天一個sigma points 更新他們的狀態
for (int i = 0; i < S; i++) 
{ 
  sigma_points.row(i) = system.f(sigma_points.row(i), control);
}

2. 加權新的sigma points 預測估計值和協方差

\[\hat z=\sum w_i\sigma_i \]

\[\hat P=\sum w_i(\sigma_i-\hat z)(\sigma_i-\hat z)^T+Q_{(過程噪聲)} \]

	const auto& Q = process_noise;

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);
    }
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
      cov_pred += weights[i] * diff * diff.transpose();
    }
    cov_pred += Q;

    mean = mean_pred;
    cov = cov_pred;

2.4 更新濾波器（measurement過程）

參考：https://blog.csdn.net/qq_34461089/article/details/88989076#commentBox