- 0. 小结
- 1: A Simple Example
- 2: Dealing with Noise
- 3: Putting it Together
- 4: State Estimation
- 5: Computing the Gain
- 6: Prediction and Update
- 7: Running the Filter
- 8: A More Realistic Model
- 9: Modifying the Estimates
- 10: Adding Velocity to the System
- 11: Linear Algebra
- 12: Prediction and Update Revisited
- 13: Sensor Fusion Intro
- 14: Sensor Fusion Example
- 15: Nonlinearity
- 16: Dealing with Nonlinearity
- 17: A Nonlinear Kalman Filter
- 18: Computing the Derivative
- 19: The Jacobian
- 20: TinyEKF
0. 小结
前面的udacity讲解了什么是扩展卡尔曼滤波，并在C++中进行了实现。但是其原理还是一知半解，通过EKF Tutorial 的学习加深理解。
1: A Simple Example
之前了解到，为什么要有扩展卡尔曼滤波器，是因为有了多源数据融合，多源数据不一定是线性的，将其进行线性变换的话，需要用卡尔曼滤波来进行处理。
这个简短教程的初衷，是作者在学习过程中没有碰到合适的入门材料而制作的，假设读者有高中程度的数学和线性代数的知识。 先从普通的线性卡尔曼滤波器着手，最终理解扩展卡尔曼滤波器。
先看一个简单的例子，比如飞机在着陆的时候，最重要的时候是其所在的高度，我们可以近似认为当前的高度，可以依据前时间段高度计算得到。
比如公式：altitude(current_time) = 0.95 * altitude(previous_time)
,如下图，这样可以根据时间差和之前的高度，来推定当前的高度：
2: Dealing with Noise
当然通过GPS或是气压计等传感器，也可以得到高度的测量值，如果传感器有恒定的误差值，将这个误差进行补正就可以得到正确值，但是实际情况是传感器的误差无法预测，随时间而变化，这叫做噪声。
observed_altitude(current_time) = altitude(current_time) + noise(current_time)
上面是不同noise时的观测值。
3: Putting it Together
现在我们有两种方式可以推测飞机的当前高度：
推测：altitude(current_time) = 0.95 * altitude(previous_time)
观测：observed_altitude(current_time) = altitude(current_time) + noise(current_time)
为了适应一般场景，将这个公式泛化一点：
x(k) = a * x(k-1)
z(k) = x(k) + v(k)
- x(k) 表示当前的系统状态，x(k-1)是上一个时间的系统状态
- a表示某个常量
- z(k) 表示当前的测量值，v(k)是当前的测量噪声
但是一般情况下，根据前时间的状态，推断当前状态的时候，不会是个完美的平滑路径，一般我们飞机下降的时候会发生颠簸，这个颠簸被定义为一种噪声。
altitude(current_time) = 0.98 * altitude(previous_time) + turbulence(current_time)
泛化后如下：
x(k) = a * x(k-1) + w(k)
w(k)叫做过程噪声(process noise)，比如颠簸加减速等。
4: State Estimation
上面得到了两个描述系统状态的公式：
x(k) = a * x(k-1) + w(k)
x(k) = z(k) - v(k) # 因为关注的是系统当前状态，所以把x(k)换到左边了
但现在的问题是，v(k)这个系统观测噪声不知道…
Fortunately, Kalman had the insight that we can estimate the state by taking into account both the current observation and the previous estimated state. 通过当前的观测值和前时间段的估算值，来估计当前的状态
上面这个弯转的有点大吧…没有经过推导，直接来到了结论!
上面有个小帽子，表示是估算值，等式左边x(k)带个小帽子，就表示估算的系统状态，即最终系统得到的值。
g是一个gain参数，也叫卡尔曼系数，用于调节观测值和估算值，即看到底哪个值更可靠一些，举两个极端例子：
参考上图，g为0的时候，测量值被无视；g=1的时候，完全信任测量值。实际情况肯定是位于这两个极端之间。
5: Computing the Gain
我们得到了这样的一个公式：
可以通过之前的估算值和当前的测量值，计算得到当前的估算值，但问题是g该如何计算呢。答案是间接地从噪声noise中计算。
我们不知道某个观测值的噪声，但是知道其平均噪声，一般传感器公开的精度数据，能近似知道噪声值，我们称之为r，这个r没有下标，因为它不随时间而变化，属于传感器的一个属性。
所以我们利用r可以计算当前的g(k):
这个结论也太突然吧…
p(k)是一个通过递归计算得到的预测误差：
