这次文章打算写关于无损滤波器的文章。
封面中，我们是在固定传感器的情况下，用lidar和radar跟踪小车的位置。红色和蓝色点都是传感器的值，而绿色的点就是经过ukf的结果。这里，要用到非线性滤波的是radar， 因为它的传感器值是需要非线性转换的。 而lidar则不需要。 指的注意的是，非线性和线性都是相对于我们取的状态变量而言的。比如，我们的状态空间里定义的变量恰好就都是从radar 直接不通过任何转换就可以提取的，那radar在这里就不需要非线性转换，而lidar 可能需要。
事实上，我应该先写经典卡尔曼滤波器Kalman filter 和扩展卡尔曼滤波器Extended kalman filter的。
不过，正好刚完成了无损滤波器unscented kalman filter project。 所以打算先写完关于UKF的文章，因为现在记忆还比较新鲜。
首先来个代码传送门。
https://github.com/Fred159/Udacity-Term2-Project2-UKF
首先简单介绍一下KF,EKF和UKF的区别。
以诞生时间来算的话，KF是大哥，EKF是二哥，UKF是三弟。
以功能来算的话，KF大哥的诞生是为了处理线性递归问题。KF大哥的诞生解决了很多以前没能好好解决的问题，有时也被称作最优滤波器，从此名震江湖。KF大哥确实是划时代的滤波器，但是因为KF是解决线性回归问题的滤波器，他在处理非线性系统的问题上不是很给力。所以，有个叫EKF的二弟总结了KF大哥的优点，并且凭借偏导数和雅克比矩阵的能力，优化了KF大哥处理非线性问题的性能。不过因为偏导数高阶导数省略问题和雅克比矩阵计算难度的问题，让EKF的效果不是很好。 再然后，三弟UKF就出现了。 UKF按一定距离取sigma points 的方式，然后让这些sigma points 按照状态转移矩阵更新一下sigma points状态的预测值，再然后根据sigma points 算出权重，最终计算出x的预测值。测量值更新的时候，（需要计算非线性的）再得到传感器信息之前，会再利用预测后的sigma points和权重，根据测量转移矩阵得到测量值的预测值。这样测量误差z_error就能根据z_传感器值 - z预测值得到。之后就比较简单了， 先计算出correlation matrix，再计算Kalman gain， 在更新x_和P_就可以了（因为他们会在下一次计算的时候用到）。
总的来说。
KF处理线性模型
EKF 通过雅克比和偏导数近似(处理）非线性模型，但是忽略了高阶导数
UKF 通过定义sigma点的方式近似非线性模型，因为没有用雅克比和偏导数，让计算变得更加简单，同时也没有忽略高阶导数项。所以相对EKF,UKF计算量少，且效果好。唯一的问题就是，看着太复杂。（实际上也复杂，所以我们在下面的文章详细讨论一下）
UKF的理念
跟KF，EKF一样，UKF也是通过预测，状态转移矩阵，预测测量值，更新测量值，再根据预测的测量值和实际的测量值之间的差值求出kalman gain，最后根据kalman gain 获得准确的状态向量和协方差。
不一样，且很不一样的就是会出现sigma points~！
下图为UKF的整个算法的流程。prediction方面有三步，update有两步，其实还有一部分，我认为要在prediction前面，就是初始化。不过这不重要~
UKF 大体算法流程
那么，我们看看UKF的sigma points 为什么能解决EKF 不能解决的问题。（个人见解，有指导意见，一定洗耳恭听）
（下图来自优达学城）
非线性情况下如何预测下个状态的X和P？
以下这段为个人理解，
知道下个状态的P（covariance）就意味着我们知道整个数据的误差范围是圆的还是椭圆形的还是完全不规则形的。但是，非线性情况下，我们是不能预测下一个状态里面的P的大小的。就算有也会像EKF一样，忽略非线性模型的高阶导数的影响，这样务必会导致P的误差范围变大。同时非线性还导致了x的计算本身都有较大的误差。
所以UKF就选择了不一样的方法。它生成了一些点，来近似非线性。由这些点来决定实际X和P的取值范围。感觉有点像粒子滤波器的概念，但还有些不同，因为UKF里的sigma points的生成并没有概率的问题。UKF的sigma points 就是把不能解决的非线性单个变量的不确定性，用多个sigma points的不确定性近似了。
UKF具体流程-Prediction-生成sigma points
下面讨论如何生成UKF的sigma points。
下图的公式为生成sigma points的公式。 第一列就是x现在的值，也就是从上一个状态接手的x值。第二列和第三列的公式中 lambda 是一个数字。具体算法是，lambda = 3-nx。是个经验公式。这里nx就是状态变量的个数。 如果我们有5个状态需要测量，那么nx就等于5。 那么根据下面公式就可以得到[5x11]的矩阵了。生成的矩阵代表的含义就是，按照一定规律生成了环绕在x周边的10个点。 由这10个点的平均值定义x的实际值（见下两图）。（事实上，lambda 表现的是sigma points离x的距离。）
sigma points之前
sigma points 定义x的值
具体代码C++ 实现如下。
MatrixXd UKF::GenerateSigmaPoints(VectorXd x, MatrixXd P, double lambda, int n_sig) {
int n = x.size();
//create sigma point matrix
MatrixXd Xsig = MatrixXd( n, n_sig );
//calculate square root of P
MatrixXd A = P.llt().matrixL();
Xsig.col(0) = x;
double lambda_plue_n_x_sqrt = sqrt(lambda + n);
for (int i = 0; i < n; i++){
Xsig.col( i + 1 ) = x + lambda_plue_n_x_sqrt * A.col(i);
Xsig.col( i + 1 + n ) = x - lambda_plue_n_x_sqrt * A.col(i);
}
return Xsig;
}
UKF具体流程-Prediction-生成Augmented sigma points
什么叫augmented？ augmented 增广的；增音的；扩张的。 我理解为增加扩展。为什么叫扩展。因为我们的状态方程里面是有噪声vk的。当这个vk对我们的状态转移矩阵有影响的话，我们需要讲这个噪声vk考虑到我们的状态转移矩阵里面的。所以，我们同时也把vk当作一种状态（噪声状态）放进我们的状态变量空间里。
UKF 状态方程
也就是说，按照上面的介绍中说道，假设原来的状态变量个数是5个。那么由于还要顾及vak和v_phik的影响，要把这两个噪声也放进状态变量里。
噪声对process 模型的影响
5个状态=》7个状态。 5个原来的状态+2个噪声状态。
原状态
扩展状态
其实在写代码的时候，直接按照扩展后的状态写就可以了
//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);
}
UKF具体流程-Prediction-prediction state
现在我们生成了很多sigma points ，那么因为物体会按一定规律移动，所以我们需要预测物体的下一个状态。这里就是根据状态转移矩阵来计算的。 状态转移矩阵的构建阶段就是，我们对物理现象的建模过程这里就不多阐述。总之就是一套公式x_k+1 = f(xk,nk)。
本次举例当中的代码如下
//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);
//predicted state values
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 point 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;
}
总之我们现在用f(x)获得了很多预测后的sigma points的值。那么现在就需要整合所有的sigma points计算实际预测的xpred 和 P_pred了。
UKF具体流程-Prediction-Predicted mean and covariance
我们现在有很多预测后的sigma points。那么我们需要实际预测的xpred 和 P_pred了。
怎么办？
我们按照一定规律去计算个个sigma points的权重，并且根据这个权重计算xpred 和 P_pred。那么具体公式如下。代入就好。这里需要解释就是lambda+na这项在生成sigma points的时候，就出现过。他代表的意思就是，sigma points 距离x越远，权重就越小。
预测平均值和协方差的值的公式
具体代码如下。这里需要注意的就是，weight刚开始的时候，是要被初始化的。且weight的第一个值的计算方法和其他不一样哦。 因为weight的第一个代表的是我们要找的x值。
// 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;
}
//predicted 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);
}
//predicted 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() ;
}
UKF具体流程-Measurement Prediction-radar
在得到具体的一个数字的x和P的预测值之后，我们需要利用求出来的x预测值来计算z_预测值。z代表测量值。为了计算我们预测的z和实际的传感器数据z的误差（因为这个要在以后的计算中用到），我们需要通过测量转移矩阵把状态空间向量和传感器可得到的数据关联起来。也就是说，通过状态空间向量x测量转移矩阵=z_预测值。下图中，x_k+1就是我们在上一个步骤中的sigma points。
z_k+1就是我们这里求的z_预测值。
注意，这里z_k+1只是我们计算出来的，并不是来自传感器的数据。
状态转移矩阵h（x_k+1)
这里要注意的是，状态空间向量是7维的，而传感器radar 可读取的数据为3个，需要注意测量转移矩阵的大小。
需要注意的是矩阵大小的变换
预测值prediction有一定的公式，如下图。
先利用上一步sigma points预测值（也就是15列sigma points）挨个求出相对应的测量值预测值。这样我们可以得到15列相应的测量值。这个值就是下图中的z_k+1|k,i 。
这里又一次出现了wi 。 wi 就是上一步中用来求x平均值的权重。这里我们可以不需要额外的计算。
把每一个wi（1x15列）和对应的每一个z_k+1|k,i （3x15列）相乘，最终得到一个值，也就是z的预测值。
这里w_k+1 是measurement model 里面的noise。他对系统没有非线性影响，所以也不用被扩展测量向量空间，只需要简单的相加就可以了。
然后计算predicted measurement covariance matrix。按照一下公式计算就可以。同样，因为R也是因为没有非线性影响，所以也可以直接相加。
Predict measurement noise and covariance
//transform sigma points into measurement space
for (int i = 0; i < 2 * n_aug + 1; i++) { //2n+1 simga points
// extract values for better readibility
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
VectorXd z_pred = VectorXd(n_z);
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug+1; i++) {
z_pred = z_pred + weights(i) * Zsig.col(i);
}
//innovation covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);
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();
}
//add measurement noise covariance matrix
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;
UKF具体流程-Measurement Prediction-Update radar
这是UKF的最后阶段。这里我们最终根据x预测值，和z预测值求出kalman gain和cross-correlation function然后最终更新state 和协方差。都是单纯的计算。而且state update和covariance matrix update都是跟KF一样的. UKF独有的计算有kalman gain和cross-correlation function。不过也都是单纯的计算。这里我想说一下，我理解的cross-correlation function的作用。 通过式子我们可以看出求cross-correlation function的内部结构。他需要每个预测的sigma points 和x预测值的差和每个sigma point 预测的测量值和z预测值的差 。也就是说，cross-correlation 这个方程会根据sigma points和预测值之间的差来平衡kalman gain。 而kalman gain里面不仅用到cross-correlation function，还会用到测量值协方差预测值。这样kalman gain就可以利用每个预测的sigma points 和x预测值的差和每个sigma point 预测的测量值和z预测值的差，来平衡模型的预测准确度和传感器的预测准确度。（kalman gain 就是一种权重）
计算卡尔曼增益，交叉相关计算和状态，协方差update
代码实现如下
//calculate cross correlation matrix
Tc.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;
// 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;
Tc = Tc + weights(i) * x_diff * z_diff.transpose();
}
//Kalman gain K;
MatrixXd K = Tc * S.inverse();
//residual
VectorXd z_diff = z - 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;
//update state mean and covariance matrix
x = x + K * z_diff;
P = P - K*S*K.transpose();
总结
事实上，这个project 是融合lidar 和radar的算法。说是融合，但说白了就是，不同时间段处理不同传感器input而已。因为这个input的不一样决定了，里面运行的代码是ukf还是ekf。像在这个project 里面，因为lidar 是线性的，所以就不需要用到UKF ，而radar因为传感器获取的数据种类就要求了它要用UKF来实现（当然EKF也可以，但是精度低而已）。
这里就不详细阐述了。 具体参考我的github代码。
开发环境
开发环境把， 我是这么做的。 也是开个传送门。这是我用docker来完成udacity 的project的。大家如果不懂可以参考一下。其实刚开始还是挺麻烦的。
https://medium.com/@flaaud159/udacity-self-driving-car-term2-extended-kalman-filter-project-environment-setting-6861f81cebadmedium.com
写了好多，不过这个项目还是仿真平台上做的。之后要做的事情就是搬到实践去
