参考概率机器人，结合基于激光做目标跟踪的应用，对KF和EKF做一个简单的记录。

运动学模型

一 速度运动模型

这个模型多用于机器人控制，具体为给定两个速度——旋转速度 $\omega$ 和平移速度 $v$，来控制机器人（这两个量是控制量，是已知的）。因此输入为： \(\textbf{u}_t=\left[v_t,\omega_t\right]^T\)

类似于自行车模型，我们认为从$t-1$时刻到$t$时刻，机器人运动的半径为（不包括直线运动，即$\omega=0$的情况）： \(r=\left| \frac{v}{\omega}\right|\)

对于机器人的某时刻位姿$(x,y,\theta)$，如图所示，显然圆的中心位置：$x_c=x-\frac{v}{\omega}\sin \theta$, $y_c=y+\frac{v}{\omega}\cos \theta$, 因此递推关系式为：

二 里程计模型

里程计模型是跟踪所需要使用的模型，其适用的情况是，我们有传感器去实时测量机器人的某些信息，比如位置或者速度，这些量是测量量，不同于速度模型里面的$v$和$w$是控制量。这个模型将机器人每个时间间隔的运动分解为——初始旋转、平移、再旋转三个步骤。我们这里还要进一步使用简化版的里程计模型，只保留一个旋转，状态向量为$[x,y,\theta]$。递推公式为：

卡尔曼滤波

这里使用里程计模型，并统一以基于激光雷达的目标跟踪为例。

一 KF

经典的卡尔曼滤波基于线性模型和高斯分布假设展开。其实质是观测信息与预测推断的加权和。这里我们设置状态向量为$[x, y, vx, vy]$，而观测只有$[x, y]$。 其公式如下(公式中的$x$表示的是状态向量，而非坐标$x$)：

$\textbf{A}$ 为状态转移矩阵。对于当前例子，设置为$[1, 0, \varDelta t, 0; 0, 1, 0, \varDelta t; 0, 0, 1, 0; 0, 0, 0, 1]$。恒速度模型。

$\textbf{Q}$ 为误差，因为模型并不能完全精准地刻画实际，比如我们恒速度模型中，加速度带来的偏差就属于$Q$之内，其他环境误差则太小可忽略不计。由此恒速度模型未考量的加速度如下： 上式中加速度的期望$\sigma x$和$\sigma y$并不能准确得到，因为目标物加速度的表达式不可知，因此可按照典型情况设置一个值，比如汽车的加速度一般约为$2m/s^2$; $\textbf{H}$ 观测矩阵，$z = Hx$,状态与观测的转换，对于激光观测的位置$[x,y]$，$H = [1,0,0,0;0,1,0,0]$

$\textbf{R}$ 观测误差的协方差，一般为激光数据误差和检测误差的综合。这里设置为0.5

$\textbf{P}$ 协方差，表示的是卡尔曼得到的状态的不确定程度。初始化的时候由于对速度的两个分量是未知的，因此对应设置一个很大的数，表示不确信：$P = [1, 0, 0, 0; 0, 1, 0, 0;0, 0, 1000, 0; 0, 0, 0, 1000]$;

二 EKF

EKF是为了解决KF不能适用于非线性系统而发展出来的，但还认为是基于高斯分布。回顾里程计模型，$x,y$的递推关系中多了三角函数，这时候就不能直接得到转移矩阵$A$。EKF的做法是将递推函数$y=f(x)$在均值点使用泰勒展开线性化：但其实高斯系统经过非线性变化之后就不一定是满足高斯分布的了，但这个时候也没办法。所以说EKF的效果一定程度上取决于非线性变化后分布与高斯分布的接近程度了。若线性化之后，如果均值点处展开的$p(y)$分布和高斯分布比较接近,EKF就会取得不错的效果，否则就不甚理想。

\(f(\textbf{x}_{t-1}, \textbf{u}_t)\approx f(\mu _{t-1})+\textbf{F}_t(\textbf{x}_{t-1}-\mu_{t-1})\)

上面的$\mu$指的就是$x$的均值点，这个均值点是什么呢，是$x$分布的期望。在速度运动模型中，输入控制量为速度，因此可以递推求得理论位置$xy$等，但是实际是有$0$均值高斯噪声的，所以这个理论值就是我要的均值。但是在里程计模型中，理想值并不能实际知道，因为观测和预测都有各自的误差，所以这里的$\mu$就用的是上一帧卡尔曼的融合输出值$x_{t-1}$，因为这个最靠近真实值。 这样之后，上述的卡尔曼递推公式中， $A = Ft$ 。这个怎么理解呢，以KF来解释，不考虑噪声的情况下，预测实质就是：$x_t=Ax_{t-1}$。

$A$可以看做一个线性方程的斜率，现在我们的递推关系变成非线性函数$f$，将其线性化之后，上式中第一项 $f(\mu)$ 是常数项，所以斜率就是$F$。

在我们的例子中，引入$\theta$之后，只有预测步骤是非线性的，观测矩阵仍然是线性的没变。很多实例中，观测到状态向量的转移也是非线性的，假设为$g(x)$，则需要同样的上述步骤线性化，然后$H = Gk$。

参数设置参考KF即可:

特殊情况

由于激光不能准确观测到障碍物的正确朝向（就是能知道box姿态，但是不知道车头朝向哪边，因此无法在$\theta_1-\theta_4$中选择），如下图，需要借助物体运动方向才能确定到底是哪个角度。

所以需要加一个trick：跑EKF的同时，嵌入一个KF一起并行，因为KF能自动得到$v_x,v_y$，因此，对于激光观测得到的四个角度，根据$v_x,v_y$的正负值筛选出正确的朝向角。每一个跟踪子在刚开始跟踪的前五帧，此时KF还不够稳定，暂时不要用角度输入，即将$R$矩阵朝向角对应那一项设为无穷大或一个很大的值，屏蔽掉角度输入:

结果分析

上图横坐标为帧数$(0.167s/frame)$，纵坐标为速度$(m/s)$。红色圆圈为惯导给出的参考速度，蓝色线条为KF的结果，绿色为EKF。可以看到，速度平稳的情况下，EKF的时延更短，但因此系统的震荡也略微偏大了一些。当然实际跟踪情况也取决于检测的好坏，因此两者在检测效果不稳定的情况下效果都会下降。