無損卡爾曼濾波UKF(3)-預測-生成Sigma點
- 2020 年 3 月 11 日
- 筆記
無損卡爾曼濾波UKF（3）-預測-生成Sigma點
1 選擇創建Sigma點
A 根據
已知上一個時間戳迭代出來的
後驗狀態 (x_{k|k}) 和後驗協方差矩陣 (P_{k|k})
他們代表當前狀態的分布。
Sigma點的數量取決於狀態向量的維度
(n_{sigma} = 2cdot 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點
考慮到雜訊的影響？？
- 擴充狀態的平均值中添加了兩個雜訊值。
- 縱向加速度項和角加速度項。均值為0 ，一定方差的正態分布。
- 他們的平均值為零，因此在平均狀態的Sigma點，將他們的值設置為零。
- 用零填充擴充的協方差矩陣。
- 然後，使用topLeftcorner函數設置擴充的協方差矩陣的左上塊。
- 方差放入增強矩陣的右下塊。 該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; }
