無損卡爾曼濾波UKF（3）-預測-生成Sigma點

1 選擇創建Sigma點

A 根據

已知上一個時間戳迭代出來的

后驗狀態 \(x_{k|k}\) 和后驗協方差矩陣 \(P_{k|k}\)

他們代表當前狀態的分布。

Sigma點的數量取決於狀態向量的維度

\(n_{\sigma} = 2\cdot n_x + 1\)

如果以兩個維度的狀態向量為例。就可以生成五個sigma點。

\(X_{k|k} = [P1,P2,P3,P4,P5]\)

矩陣的每一列都代表一個Sigma點。

\(X_{k|k} = [x_{k|k},x_{k|k}+\sqrt[2]{(\lambda+n_x)P_{k|k}},x_{k|k}-\sqrt[2]{(\lambda+n_x)P_{k|k}} ]\)

關於Lambda,是一個設計的參數，一般情況下，按下面的設置，效果還不錯

\(\lambda = 3 - n_x\)

求矩陣的平方根 => 找到矩陣A

\(A = \sqrt[2]{P_{k|k}} <= A^T A = P_{k|k}\)

第一個點就是狀態向量的均值。

如果Lambda值大,Sigma點會距離均值點遠一些。

生成Sigma點的代碼（1）

/*
    根據上述公式，完成生成Sigma點的函數
*/
void UKF::GenerateSigmaPoints(MatrixXd* Xsig_out) {

  // 設置狀態向量的維度
  int n_x = 5;

  // 定義傳播參數
  double lambda = 3 - n_x;

  // 給定一個樣例狀態
  VectorXd x = VectorXd(n_x);
  x <<   5.7441,
         1.3800,
         2.2049,
         0.5015,
         0.3528;

  // 給定一個樣例狀態的協方差矩陣
  MatrixXd P = MatrixXd(n_x, n_x);
  P <<     0.0043,   -0.0013,    0.0030,   -0.0022,   -0.0020,
          -0.0013,    0.0077,    0.0011,    0.0071,    0.0060,
           0.0030,    0.0011,    0.0054,    0.0007,    0.0008,
          -0.0022,    0.0071,    0.0007,    0.0098,    0.0100,
          -0.0020,    0.0060,    0.0008,    0.0100,    0.0123;

  // 創建Sigma點的矩陣、一列代表一個Sigma點、
  MatrixXd Xsig = MatrixXd(n_x, 2 * n_x + 1);

  // 計算矩陣P的平方根
  MatrixXd A = P.llt().matrixL();

  // 設置Sigma矩陣的第一列，一列代表一個Sigma點
  Xsig.col(0) = x;

  // 設置Sigma矩陣剩下的點
  for (int i = 0; i < n_x; ++i) {
    Xsig.col(i+1)     = x + sqrt(lambda+n_x) * A.col(i);
    Xsig.col(i+1+n_x) = x - sqrt(lambda+n_x) * A.col(i);
  }
  
  // 打印結果
  std::cout << "Xsig = " << std::endl << Xsig << std::endl;
  
  // 返回結果
  *Xsig_out = Xsig;
}

B 擴充后創建Sigma點

考慮到噪聲的影響？？

1. 擴充狀態的平均值中添加了兩個噪聲值。
2. 縱向加速度項和角加速度項。均值為0 ，一定方差的正態分布。
3. 他們的平均值為零，因此在平均狀態的Sigma點，將他們的值設置為零。
4. 用零填充擴充的協方差矩陣。
5. 然后，使用topLeftcorner函數設置擴充的協方差矩陣的左上塊。
6. 方差放入增強矩陣的右下塊。 該2x2塊對應於矩陣QQ。

除了這次創建了更多的sigma點，其余部分與以前完全相同。

void UKF::AugmentedSigmaPoints(MatrixXd* Xsig_out) {

  // 維數
  int n_x = 5;

  // 擴展后維數為7
  int n_aug = 7;

  // Process noise standard deviation longitudinal acceleration in m/s^2
  double std_a = 0.2;

  // Process noise standard deviation yaw acceleration in rad/s^2
  double std_yawdd = 0.2;

  // 定義傳播參數
  double lambda = 3 - n_aug;

  VectorXd x = VectorXd(n_x);
  x <<   5.7441,
         1.3800,
         2.2049,
         0.5015,
         0.3528;

  MatrixXd P = MatrixXd(n_x, n_x);
  P <<     0.0043,   -0.0013,    0.0030,   -0.0022,   -0.0020,
          -0.0013,    0.0077,    0.0011,    0.0071,    0.0060,
           0.0030,    0.0011,    0.0054,    0.0007,    0.0008,
          -0.0022,    0.0071,    0.0007,    0.0098,    0.0100,
          -0.0020,    0.0060,    0.0008,    0.0100,    0.0123;

  // 創建擴充后的平均值向量
  VectorXd x_aug = VectorXd(7);

  // 創建擴充后的狀態協方差矩陣
  MatrixXd P_aug = MatrixXd(7, 7);

  // 創建擴充后的Sigma矩陣

  MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);

  // 設置擴充后的平均值向量的參數值
  x_aug.head(5) = x;
  x_aug(5) = 0;
  x_aug(6) = 0;

  // 設置擴充后的狀態協方差矩陣
  P_aug.fill(0.0);
  P_aug.topLeftCorner(5,5) = P;
  P_aug(5,5) = std_a*std_a;
  P_aug(6,6) = std_yawdd*std_yawdd;

  // 求P的平方根
  MatrixXd L = P_aug.llt().matrixL();

  // 設置Sigma矩陣其他位置的值
  Xsig_aug.col(0)  = x_aug;
  for (int i = 0; i< n_aug; ++i) {
    Xsig_aug.col(i+1)       = x_aug + sqrt(lambda+n_aug) * L.col(i);
    Xsig_aug.col(i+1+n_aug) = x_aug - sqrt(lambda+n_aug) * L.col(i);
  }
  
  std::cout << "Xsig_aug = " << std::endl << Xsig_aug << std::endl;

  *Xsig_out = Xsig_aug;
}