原因
传感器存在误差——需要跟踪——卡尔曼滤波
由于传感器本身的特性，任何测量结果都是有误差的。
以障碍物检测为例，如果直接使用传感器的测量结果，在车辆颠簸时，可能会造成障碍物测量结果的突变，这对无人车的感知来说是不可接受的。
在传感器测量结果的基础上，进行跟踪，以此来保证障碍物的位置、速度等信息不会发生突变。
应用
障碍物跟踪、车道线跟踪、障碍物预测以及定位等领域；
原理
线性系统，且参数是符合高斯分布，即完全可以被均值和协方差参数化:X∼N(μ, σ²)。
$ x_2=w_1*x_{pre}+w_2*z_2 $
预测状态（prediction）与观测状态(measurement)分别加权求和作为新时刻的状态向量。
- 预测根据前一周期的状态和运动学方程求得；
- 观测通过传感器测得的当前周期参数得到；
- 预测与观测均成高斯分布，两个高斯分布的乘积就是加权后的结果。新的高斯分布方差最小，确定性也最高。（两个不那么确定的分布，最终得到了一个相对确定的分布，这是卡尔曼滤波的一直被推崇的原因）
7大公式
prediction：
(状态协方差矩阵，表示系统不确定度)
measurement update:
（实际观测到的测量值 z 与预测值 x' 之间差值 y）
（当前状态向量 x 的更新）
（根据卡尔曼增益，更新了系统的不确定度 P）
其中，
F（state transition matrix）：状态转移矩阵(运动学推导)；
B：控制输入矩阵；
u：引起运动的输入向量（如油门、刹车、转角）；
w：过程向量中的噪声因素，符合高斯分布，w∼N(0,Q)；
P（state covariance matrix）：在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小；
Q（process covariance matrix）：过程噪声，即无法用 x'=Fx+u 表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的；（由于 Q 对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将 Q 设置为单位矩阵参与运算）
H（Measurement Matrix）：测量矩阵，由于传感器的测量值不一定就是直接的状态向量，如毫米波雷达是角度和位置信息，这里H就是转换矩阵，将预测向量转为测量数据；
R （measurement covariance matrix）：测量噪声矩阵，表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值；
S 只是为了简化公式，写的一个临时变量；
K（Kalman Gain）：卡尔曼增益，也即y的权重。
问题：针对线性系统，且符合高斯分布。
EKF扩展卡尔曼滤波
当状态向量与测量量之间映射关系是非线性时，这将使得卡尔曼滤波的过程和测量符合高斯分布的假设无效。
如毫米波雷达，返回数据是基于极坐标系，包含：
- ρ /Range (从原点到此的距离)；
- ϕ /bearing (ρ 和 x 的夹角)；
- ρ˙：接近率 / 距离变化率；
此时，H转移矩阵（状态向量向测量数据转换），也即笛卡尔坐标向极坐标映射：
这个映射就是非线性函数。
EKF使用局部线性逼近非线性模型，通过计算当前状态估计的一阶泰勒展开得出。一阶的逼近也叫雅克比矩阵。
如对上泰勒展开（在均值u处，本例为0），
求出雅克比矩阵如下：
对比公式
问题：非线性系统线性化省略高阶项，导致误差；
另外当状态向量复杂时候，求取雅克比矩阵难度也会上升。
