在上一节的扩展卡尔曼滤波跟踪系统中，有两个缺陷：
-
系统采用恒速模型：假定行人沿直线运动；实际应用中，出现曲线运动时，预估不够准确。
-
每次测量都需要计算雅可比矩阵，很耗资源。
当问题一旦变得复杂，预测和测量模型高度非线性化时，扩展卡尔曼滤波 EKF 计算量就变得十分不可控，效果表现也较差；
为解决这些问题，学习一个新的状态估计算法 --无损卡尔曼滤波(Unscented Kalman Filters)
无损卡尔曼滤波基本原理与之前的通用卡尔曼滤波完全一样，还是包括预测和测量更新两部分。不同的是，采取新的运动模型(CTRV)，对非线性化的处理(基于统计采样)。
问题 1：运动模型
在扩展卡尔曼滤波中，我们用的是恒速模型(constant velocity, CV)，恒速模型是物体追踪中最常见的模型；其他常见的运动模型有：
CTRV：constant turn rate and velocity magnitude model
CTRA：constant turn rate and acceleration
CSAV：constant steering angle and velocity
CCA：constant curvature and acceleration
在无损卡尔曼滤波中，我们采用 CTRV 模型，CTRV 模型更适合真实的交通场景；
1.1 状态向量
CTRV 模型的状态向量包括，
General State Vector
$$x = \begin{bmatrix}
p_x\ p_y\ v\ \psi\ \dot{\psi}
\end{bmatrix}
$$
$p_x,p_y$: 行人位置
$v$: 速度,
$\psi$: 偏航角
$\dot{\psi}$: 偏航角速度
1.2 过程模型
已知 k 时刻的状态向量 $xk$，要预测 k + 1 时刻的状态向量 $x{k+1}$, $v_k$ 是过程噪声:
$$
x_{k+1} = f(x_k, v_k)
$$
方程式中，过程模型 f，根据 $x_k 和 v k 的状态，预测 x {k+1}$
先实现过程模型确定性部分
由于 CTRV 模型是非线性的，通用更新方法是
$$
x_{k+1} = f(xk) + \int{xk}^{x{k+1}}\dot{x}
$$
也即求出状态向量 x 的微分方程，时间变化 dt，就可以利用积分更新过程模型
这是一个非常通用，也非常有用的方法，可以用来推演很多真实世界问题的过程模型。
微分方程如下：
求积分得到过程模型：
它是一个函数，根据 k 时刻的状态，求出了 k + 1 时刻的状态。这里有个问题，当角速度为 0 时，会出现除数为 0；
这是个正常现象，意味着车辆 (或行人) 在沿直线运动，把该情景做特殊处理，当 $\dot{\psi}$= 0 时，过程函数简化如下：
$$x_{k+1} = \begin{bmatrix}
v_kcos(\psi_k)dt\
v_ksin(\psi_k)dt\
v_k\
\psi\
0
\end{bmatrix}
$$
这是过程模型的确定部分，下面求过程模型的随机部分。
过程模型随机部分
过程模型的随机部分，用一个二维噪音向量 $v_k$ 描述，不确定性 $v_k$ 由两个独立的标量噪音过程组成：
纵向加速度噪音 $v_a$：影响车辆的纵向速度，并且在每个时间步改变它的值；纵向加速度是随机分布的白噪声；
角加速度 $v_{\dot{\dot\psi}}$: 也是一个均值为 0 的正态分布白噪声；
假设角加速度和直线加速度，都是常量，将它们添加到过程函数中：
综上，过程模型公式为：
当 $\dot\psi \not=0$
$$x_{k+1} = x_k +
\begin{bmatrix}
\frac{v_k}{\dot\psi_k}(sin(\psi_k + \dot{\psi}dt) - sin(\psi_k))\
\frac{v_k}{\dot\psi_k}(-cos(\psi_k + \dot\psi_kdt) + cos(\psik))\
0\
\dot{\psi}dt\
0
\end{bmatrix} + \begin{bmatrix}
\frac{1}{2}(dt)^2v{a,k}cos(\psik)\
\frac{1}{2}(dt)^2v{a,k}sin(\psik)\
dt{\cdot}v{a,k}\
\frac{1}{2}(dt)^2{\cdot}v{\dot{\dot{\psi}},k}\
dt{\cdot}v{\dot{\dot{\psi}},k}\
\end{bmatrix}
$$
当 $\dot\psi =0$，即车辆沿直线运动时，模型公式为
$$x_{k+1} = x_k +
\begin{bmatrix}
v_kcos(\psi_k)dt\
v_ksin(\psi_k)dt\
vk\
\psi\
0
\end{bmatrix}+ \begin{bmatrix}
\frac{1}{2}(dt)^2v{a,k}cos(\psik)\
\frac{1}{2}(dt)^2v{a,k}sin(\psik)\
dt{\cdot}v{a,k}\
\frac{1}{2}(dt)^2{\cdot}v{\dot{\dot{\psi}},k}\
dt{\cdot}v{\dot{\dot{\psi}},k}\
\end{bmatrix}
$$
问题 2：如何处理非线性模型？
我们已经从 CTRV 模型中推演出了一个过程模型，这个模型考虑到了直线行驶或拐弯的可能性，这在实际应用追踪自行车时有不错的效果。
这个过程模型，是非线性的。经过非线性转换后，结果已经不是一个高斯分布，一个不规则的分布
EKF 的思想是寻找一个线性函数模型来近似这个非线性模型，而 UKF 的思想是
== 寻找一个与真实分布近似的高斯分布 ==
问题在于，如何才能找到这个均值向量和协方差矩阵？
这个寻找的方法是，无损变换。
使用非线性函数进行整个分布的转换是很难的，但转换状态空间的单个点非常容易。
使用一定方法产生一组代表性 sigma point 点集，这些 sigma point 采样点能够代表当前的分布，然后将这些点通过非线性函数变换到一个新的空间（预测空间），
然后基于这些新的 sigma point 计算出一个新的高斯分布。
现在新的高斯分布，均值和协方差与真正预测的分布相同(可以给出有用的近似)。
值得一提的是，同样的方法也适用于线性案例，如果过程具备线性特征,sigma 点方法可以提供和标准通用卡尔曼滤波器一样的解。
但这里最好不要使用 sigma 点，因为它们计算时间太长
无损预测分三步：
step1，选择 sigma 点的办法；
以两个参数为例(便于图示化)
sigma 点个数，一般由状态向量维度决定，num=2*n+1
取 sigma 点方法：
$$X{k|k} = \Bigg [x{k|k} \qquad x_{k|k}+\sqrt{(\lambda+nx)P{k|k}} \qquad x_{k|k}-\sqrt{(\lambda+nx)P{k|k}} \Bigg]
$$
第一个点就是均值点，
$x_{k|k}+\sqrt{(\lambda+nx)P{k|k}}$ 表示 1 到 $n_x + 1 列 $
$x_{k|k}-\sqrt{(\lambda+nx)P{k|k}} 表示 n_x + 2 到 2n_x + 1 列 $
$\lambda$ 是一个超参数，$\lambda$ 越大，sigma point 就越远离分布的均值，$\lambda$ 越小，sigma point 越靠近均值。
计算
$$
AA^T = P
$$
求解上式的 A 是一个复杂的过程，但如果 P 是对角矩阵，这个求解就可以简化，实际上 P 表示对估计状态的不确定性(协方差矩阵)，状态空间各分量基本是独立的，也就是说 P 基本上就是对角矩阵，所以对 P 进行 Choesky 分解，得到的下三角矩阵就是 A。
代码实现，核心代码：
/**
* Programming assignment functions:
*/
void UKF::GenerateSigmaPoints(MatrixXd* Xsig_out) {
// set state dimension
int n_x = 5;
// define spreading parameter
double lambda = 3 - n_x;
// set example state
VectorXd x = VectorXd(n_x);
x << 5.7441,
1.3800,
2.2049,
0.5015,
0.3528;
// set example covariance matrix
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;
// create sigma point matrix
MatrixXd Xsig = MatrixXd(n_x, 2 * n_x + 1);
// calculate square root of P
MatrixXd A = P.llt().matrixL();
/**
* Student part begin
*/
// your code goes here
// calculate sigma points ...
// set sigma points as columns of matrix Xsig
Xsig.col(0) = x;
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);
}
/**
* Student part end
*/
// print result
// std::cout << "Xsig = " << std::endl << Xsig << std::endl;
// write result
*Xsig_out = Xsig;
}过程模型包括两部分，过程噪声也要考虑在内，
过程噪声也是一个非线性模型，考虑噪声过程的 sigma points 用一个增广矩阵表示
代码实现：
/**
* Programming assignment functions:
*/
void UKF::AugmentedSigmaPoints(MatrixXd* Xsig_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
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;
// define spreading parameter
double lambda = 3 - n_aug;
// set example state
VectorXd x = VectorXd(n_x);
x << 5.7441,
1.3800,
2.2049,
0.5015,
0.3528;
// create example covariance matrix
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;
// create augmented mean vector
VectorXd x_aug = VectorXd(7);
// create augmented state covariance
MatrixXd P_aug = MatrixXd(7, 7);
// create sigma point matrix
MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
/**
* Student part begin
*/
// create augmented mean state
x_aug.head(5) = x;
x_aug(5) = 0;
x_aug(6) = 0;
// create augmented covariance matrix
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;
// create square root matrix
MatrixXd L = P_aug.llt().matrixL();
// create augmented sigma points
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);
}
/**
* Student part end
*/
// print result
std::cout << "Xsig_aug = " << std::endl << Xsig_aug << std::endl;
// write result
*Xsig_out = Xsig_aug;
}step2，如何预测 sigma 点（插入过程函数就可以）;
把 step1 中产生的 sigma points 输入过程函数即可:
当 $\dot\psi \not=0$
$$x_{k+1} = x_k +
\begin{bmatrix}
\frac{v_k}{\dot\psi_k}(sin(\psi_k + \dot{\psi}dt) - sin(\psi_k))\
\frac{v_k}{\dot\psi_k}(-cos(\psi_k + \dot\psi_kdt) + cos(\psik))\
0\
\dot{\psi}dt\
0
\end{bmatrix} + \begin{bmatrix}
\frac{1}{2}(dt)^2v{a,k}cos(\psik)\
\frac{1}{2}(dt)^2v{a,k}sin(\psik)\
dt{\cdot}v{a,k}\
\frac{1}{2}(dt)^2{\cdot}v{\dot{\dot{\psi}},k}\
dt{\cdot}v{\dot{\dot{\psi}},k}\
\end{bmatrix}
$$
当 $\dot\psi =0$，即车辆沿直线运动时，模型公式为
$$x_{k+1} = x_k +
\begin{bmatrix}
v_kcos(\psi_k)dt\
v_ksin(\psi_k)dt\
vk\
\psi\
0
\end{bmatrix}+ \begin{bmatrix}
\frac{1}{2}(dt)^2v{a,k}cos(\psik)\
\frac{1}{2}(dt)^2v{a,k}sin(\psik)\
dt{\cdot}v{a,k}\
\frac{1}{2}(dt)^2{\cdot}v{\dot{\dot{\psi}},k}\
dt{\cdot}v{\dot{\dot{\psi}},k}\
\end{bmatrix}
$$
注意:
1. 输入向量是 7 维，输出向量是 5 维的;
2. 当角速度为 0 时，注意除 0 问题，模型退化成沿直线行驶；
代码实现：
/**
* Programming assignment functions:
*/
void UKF::SigmaPointPrediction(MatrixXd* Xsig_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// create example sigma point matrix
MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
Xsig_aug <<
5.7441, 5.85768, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.63052, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441, 5.7441,
1.38, 1.34566, 1.52806, 1.38, 1.38, 1.38, 1.38, 1.38, 1.41434, 1.23194, 1.38, 1.38, 1.38, 1.38, 1.38,
2.2049, 2.28414, 2.24557, 2.29582, 2.2049, 2.2049, 2.2049, 2.2049, 2.12566, 2.16423, 2.11398, 2.2049, 2.2049, 2.2049, 2.2049,
0.5015, 0.44339, 0.631886, 0.516923, 0.595227, 0.5015, 0.5015, 0.5015, 0.55961, 0.371114, 0.486077, 0.407773, 0.5015, 0.5015, 0.5015,
0.3528, 0.299973, 0.462123, 0.376339, 0.48417, 0.418721, 0.3528, 0.3528, 0.405627, 0.243477, 0.329261, 0.22143, 0.286879, 0.3528, 0.3528,
0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641, 0,
0, 0, 0, 0, 0, 0, 0, 0.34641, 0, 0, 0, 0, 0, 0, -0.34641;
// create matrix with predicted sigma points as columns
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
double delta_t = 0.1; // time diff in sec
/**
* Student part begin
*/
// predict sigma points
for (int i = 0; i< 2*n_aug+1; ++i) {
// extract values for better readability
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 yawd = Xsig_aug(4,i);
double nu_a = Xsig_aug(5,i);
double nu_yawdd = Xsig_aug(6,i);
double px_p, py_p;
// avoid division by zero
if (fabs(yawd) > 0.001) {px_p = p_x + v/yawd * ( sin (yaw + yawd*delta_t) - sin(yaw));
py_p = p_y + v/yawd * (cos(yaw) - cos(yaw+yawd*delta_t) );
} else {px_p = p_x + v*delta_t*cos(yaw);
py_p = p_y + v*delta_t*sin(yaw);
}
double v_p = v;
double yaw_p = yaw + yawd*delta_t;
double yawd_p = yawd;
// add noise
px_p = px_p + 0.5*nu_a*delta_t*delta_t * cos(yaw);
py_p = py_p + 0.5*nu_a*delta_t*delta_t * sin(yaw);
v_p = v_p + nu_a*delta_t;
yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t;
yawd_p = yawd_p + nu_yawdd*delta_t;
// write predicted sigma points into right column
Xsig_pred(0,i) = px_p;
Xsig_pred(1,i) = py_p;
Xsig_pred(2,i) = v_p;
Xsig_pred(3,i) = yaw_p;
Xsig_pred(4,i) = yawd_p;
}
/**
* Student part end
*/
// print result
std::cout << "Xsig_pred = " << std::endl << Xsig_pred << std::endl;
// write result
*Xsig_out = Xsig_pred;
}step3, 根据预测的 sigma 点，计算预测、均值和协方差；
计算方法：
注意：
对权重 $w_i$ 的定义，方法有多种，这里采用常用的：
Weights:
$wi =\frac{\lambda}{\lambda+n{a}}, i =1$
$wi =\frac{1}{2(\lambda+n{a})}, i =2...n_{a}$
Predicted Mean
$x{k+1|k} = \sum{i=1}^{n_\sigma} wi X{k+1|k,i}$
Predicted Covariance
$P{k+1|k} = \sum{i=1}^{n_\sigma} wi(X{k+1|k,i} - x{k+1|k})(X{k+1|k,i} - x_{k+1|k})^T$
实现代码：
/**
* Programming assignment functions:
*/
void UKF::PredictMeanAndCovariance(VectorXd* x_out, MatrixXd* P_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// define spreading parameter
double lambda = 3 - n_aug;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
// create vector for predicted state
VectorXd x = VectorXd(n_x);
// create covariance matrix for prediction
MatrixXd P = MatrixXd(n_x, n_x);
/**
* Student part begin
*/
// set weights
double weight_0 = lambda/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) { // 2n+1 weights
double weight = 0.5/(n_aug+lambda);
weights(i) = weight;
}
// predict state mean
x.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; ++i) { // iterate over sigma points
x = x + weights(i) * Xsig_pred.col(i);
//cout<<"print i="<<i<<endl;
//cout<<x;
}
// predict state covariance matrix
P.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; ++i) { // iterate over sigma points
// state difference
VectorXd x_diff = Xsig_pred.col(i) - x;
// angle normalization
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 = P + weights(i) * x_diff * x_diff.transpose() ;}
/**
* Student part end
*/
// print result
std::cout << "Predicted state" << std::endl;
std::cout << x << std::endl;
std::cout << "Predicted covariance matrix" << std::endl;
std::cout << P << std::endl;
// write result
*x_out = x;
*P_out = P;
}现在已经完成过程模型的预测更新
2 测量更新
把预测状态 x 转换为测量空间的函数，叫做 测量模型；
当然，必须考虑是由哪种传感器生成的当前测量值，并使用对应的测量模型(每种传感器有相对应的测量模型)。
当测量模型是非线性时，我们必须采用非线性模型处理，类似于前面介绍的预测更新过程模型。
不过这里有个快捷技巧，可以直接重用预测过程中估计的 sigma points，这里还可以跳过扩充步骤，需要扩充是因为过程噪声，
对状态有非线性影响，
比如雷达，测量噪声具有单纯的累加效果，这里不需要扩充，测量噪声，有其他更好的办法？
这里唯一要做的就是把已经有的 sigma 点转换到测量空间，并使用这些点来计算预测测量值的均值和协方差。
注意：
使用雷达传感器，状态向量转换到测量空间，只有三个维度。
测量噪声，暂时设置 0；
1 计算估计值 (转换到测量空间) 的均值和协方差
计算方法与前面预测更新部分相似，唯一不同是多出了个测量协方差噪声
这里的测量噪声不具备非线性效应，只是单纯的累计性的，直接使用替换扩充。
Helpful Equations
State Vector
$$
_{k+1|k}=\begin{bmatrix} p_x\ p_y\ v\ \psi\ \dot{\psi} \end{bmatrix}
$$
Measurement Vector
$$
z_{k+1|k}=\begin{bmatrix} \rho\ \varphi\ \dot{\rho} \end{bmatrix}
$$
Measurement Model
$$
z{k+1|k}=h(x{k+1}) + w_{k+1}
$$
$$
\rho = \sqrt{p_x^2+p_y^2}
$$
$$
\varphi =arctan(\frac{p_y}{p_x})
$$
$$
\dot{\rho}=\frac{p_xcos(\psi)v+p_ysin(\psi)v}{\sqrt{p_x^2+p_y^2}}
$$
Predicted Measurement Mean
$$
z{k+1|k} = \sum{i=1}^{n_\sigma} wi Z{k+1|k,i}
$$
Predicted Covariance
$$
S{k+1|k} = \sum{i=1}^{n_\sigma} wi(Z{k+1|k,i} - z{k+1|k})(Z{k+1|k,i} - z_{k+1|k})^T + R
$$
$$
R = E(w_k\cdot wk^T) = \begin{bmatrix} \sigma{\rho}^2 \qquad 0\qquad0\ 0\qquad\sigma{\varphi}^2 \qquad 0\ 0\qquad0\qquad\sigma{\dot{\rho}}^2 \end{bmatrix}
$$
代码实现:
/**
* Programming assignment functions:
*/
void UKF::PredictRadarMeasurement(VectorXd* z_out, MatrixXd* S_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {weights(i) = weight;
}
// radar measurement noise standard deviation radius in m
double std_radr = 0.3;
// radar measurement noise standard deviation angle in rad
double std_radphi = 0.0175;
// radar measurement noise standard deviation radius change in m/s
double std_radrd = 0.1;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// 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);
/**
* Student part begin
*/
// transform sigma points into measurement space
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
// extract values for better readability
double p_x = Xsig_pred(0,i);
double p_y = Xsig_pred(1,i);
double v = Xsig_pred(2,i);
double yaw = Xsig_pred(3,i);
double v1 = cos(yaw)*v;
double v2 = sin(yaw)*v;
// measurement model
Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y); // r
Zsig(1,i) = atan2(p_y,p_x); // phi
Zsig(2,i) = (p_x*v1 + p_y*v2) / sqrt(p_x*p_x + p_y*p_y); // r_dot
}
// mean predicted measurement
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug+1; ++i) {z_pred = z_pred + weights(i) * Zsig.col(i);
}
// calculate mean predicted measurement
S.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
// residual
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 = S + weights(i) * z_diff * z_diff.transpose();}
// calculate innovation covariance matrix S
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;
S = S + R;
/**
* Student part end
*/
// print result
std::cout << "z_pred: " << std::endl << z_pred << std::endl;
std::cout << "S: " << std::endl << S << std::endl;
// write result
*z_out = z_pred;
*S_out = S;
}2 计算卡尔曼增益，更新状态空间
最后一步，查看测量值，计算卡尔曼增益，估算新的状态空间和协方差矩阵
更新公式计算方法：
Helpful Equations
Cross-correlation Matrix：计算 sigma point 点集在状态空间和测量空间的相互关系函数，
$$
T{k+1|k} = \sum{i=1}^{n_\sigma} wi (X{k+1|k,i} - x{k+1|k})\ (Z{k+1|k,i} - z_{k+1|k})^T
$$
Kalman gain K
$$K{k+1|k} = T{k+1|k}S^{-1}_{k+1|k}$$
Update State：也就是作出最后的状态估计
$$
x{k+1|k+1} = x{k+1|k}+K{k+1|k}(z{k+1}-z_{k+1|k})
$$
Covariance Matrix Update
$$
P{k+1|k+1} = P{k+1|k}-K{k+1|k}S{k+1|k}K^T_{k+1|k}
$$
3 参数调节和一致性
最后一个问题，如何选择噪声参数，以及如何评估所选参数效果？
这不仅与无损卡尔曼滤波器相关，同样适用于贝叶斯滤波器。
在过程模型中，我们引入了过程噪声 $v_k$, 测量模型中引入了测量噪声 $w_k$，还使用过程噪声协方差矩阵和测量噪声协方差矩阵进行了噪音量化，
这些噪声的的方差，表示噪声的强度，但是这些值是怎么来的呢？
测量噪声描述了传感器测量的精确度，可以从数据手册查看其精度，假设是白噪声，服从高斯分布
选择过程噪声更难些，实际的交通环境里，对象的移动加速度噪声不是白噪音（司机可能不停踩油门和刹车），因此，这里使用强近似，
假设过程噪声是白噪声，要得到有用的值，谨记下面的规则：
== 尽量估算你所在环境中可能碰到的最大加速度，比如城市环境，这些车加速或者刹车的加速度，通常不会超过 $6m/s^2$;==
选择预期最大加速度的一半作为过程噪声，这个值到底好不好，取决于你的应用，应用对变更的快速反应是不是很重要，如果是，那就选择更高一点的过程噪声。
对提供平滑估算是不是很重要，如果是，那就选择更低一点的过程噪音。
你通常需要知道噪音参数设置是否正确，可以运行滤波器一致性检查。
Consistency:
NORMALIZED INNOVATION SQUARED(NIS)
在每个循环周期，我们估算测量的均值 $Z{k+1}|k$ 和协方差矩阵 S，实际测量值 $z{k+1}$ 落在误差椭圆范围内, 这是正常的。
但实际可能会出现这样，
低估了预测测量值的不确定性，这说明滤波器是不一致的。估算的精度比你想的要低。
而出现这样，
高估了系统的不确定性，也就是说估算的精度比你想的要高，滤波器是不一致的。
滤波器的一致性，== 是指能提供真实的估算不确定性 ==。
建议在设计滤波器时，始终检查一致性。检查方法：归一化信息平方，(Normalized Innovation Squared，NIS)
归一化，是指相对于矩阵 S 的协方差而言
NIS 是一个标量数据。
NIS 的统计知识
NIS 的分布符合卡方分布，
df：表示自由度，即测量空间的维度；
比如雷达的 df=3,
0.95 表示，在统计意义上，所有情况下有 95% 的概率，NIS 会超过 0.352；
0.05 表示，在所有情况下，有 5% 的概率，NIS 会超过 7.815；
设计滤波器时，画出 95% 这条线，即 NIS=7.815，对每个时间步 k 计算并画出 NIS 值
如果得到这样的图，是正常的，有时候会超过 95% 的线
如果得到这样的图
说明系统低估了不确定性。
如果得到这样的图
说明系统高估了不确定性，估算精度比你想的更高。
不幸的时，NIS 不会告诉你错误原因，但至少它提供了一些反馈信息，根据反馈信息可以调节误差参数。
实际测到的雷达 NIS 一致性检查：
激光 NIS 一致性检查：(95% 线不一样)
如果 UKF 是一致的，它就能提供真实的协方差矩阵。
UKF 还能估算自行车的速度，有没有雷达都可以，== 如果使用雷达，速度估算收敛的更快 ==。
可以打开关闭两个传感器，看看各自的影响。
还可以估算角速度：
角速度对无人车尤其重要，角速度时最终行为的预测指标。
UKF 三个功能：
1. 在 UKF 中，你可以输入噪声测量数据，获得周围动态对象的位置和速度的平滑估算，不会引发延时；
2. 即便使用的传感器无法直接观察，还是可以获得其他车辆的方向和速度估算。
3.UKF 能给出结果的准确度，因为它能提供每个估算的协方差矩阵；如果 UKF 已经执行了一致性检查，你就能知道协方差矩阵是真实的。
4 状态向量的初始化
Initializing the Kalman Filter
The state vector x contains
$x = [p_x, p_y, v, \psi, \dot{\psi}]$
$p_x,p_y$: 通过传感器测量获得；
For the other variables in the state vector x, you can try different initialization values to see what works best.
注：雷达测量的速度值(无人车视角)，不能直接用来初始化状态空间的速度(CTRV 中，速度是自行车的视角，速度与自行车行驶圆周相切);
协方差矩阵，可以初始化为单位矩阵。
5 扩充资源
Tracking Multiple Objects and Sensor Fusion
The below papers and resources concern tracking multiple objects, using Kalman Filters as well as other techniques!
Stereo cameras
The below papers cover various methods of using stereo camera set-ups for object detection and tracking.
Robust 3-D Motion Tracking from Stereo Images: A Model-less Method by Y.K. Yu, et. al.
Vehicle Tracking and Motion Estimation Based on Stereo Vision Sequences by A. Barth (long read)
Deep Learning-based approaches
The below papers include various deep learning-based approaches to 3D object detection and tracking.
VoxelNet: End-to-End Learning for Point Cloud Based 3D Object Detection by Y. Zhou and O. Tuzel
Other papers on Tracking Multiple Objects and Sensor Fusion
The below papers and resources concern tracking multiple objects, using Kalman Filters as well as other techniques! We have not included the abstracts here for brevity, but you should check those out first to see which of these you want to take a look at.
Multiple Object Tracking using Kalman Filter and Optical Flow by S. Shantaiya, et. al.
Tracking Multiple Moving Objects Using Unscented Kalman Filtering Techniques by X. Chen, et. al.
LIDAR-based 3D Object Perception by M. Himmelsbach, et. al
3D-LIDAR Multi Object Tracking for Autonomous Driving by A.S. Rachman (long read)
