無損卡爾曼濾波UKF(3)-預測-生成Sigma點
阿新 • • 發佈:2020-03-11
# 無損卡爾曼濾波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)
```cpp
/*
根據上述公式,完成生成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點
![](https://img2020.cnblogs.com/blog/1516652/202003/1516652-20200311150634298-1224432731.png)
![](https://img2020.cnblogs.com/blog/1516652/202003/1516652-20200311150659697-1836523367.png)
![](https://img2020.cnblogs.com/blog/1516652/202003/1516652-20200311150744779-167455881.png)
##### 考慮到噪聲的影響??
1. 擴充狀態的平均值中添加了兩個噪聲值。
1. 縱向加速度項和角加速度項。均值為0 ,一定方差的正態分佈。
1. 他們的平均值為零,因此在平均狀態的Sigma點,將他們的值設定為零。
1. 用零填充擴充的協方差矩陣。
1. 然後,使用topLeftcorner函式設定擴充的協方差矩陣的左上塊。
1. 方差放入增強矩陣的右下塊。 該2x2塊對應於矩陣QQ。
除了這次建立了更多的sigma點,其餘部分與以前完全相同。
```cpp
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;
}
```